void motor_set_dir_all(motor_dir_t dir)
{
motor_set_dir(MOTOR_1, dir);
motor_set_dir(MOTOR_2, dir);
motor_set_dir(MOTOR_3, dir);
motor_set_dir(MOTOR_4, dir);
}
void motor_set(motor_chan_t motor, int16_t speed)
{
if (speed < 0) {
speed = speed * -1;
motor_set_dir(motor, MOTOR_FW);
} else {
motor_set_dir(motor, MOTOR_BW);
}
speed = (speed > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;
motor_set_speed(motor, speed);
}
void motor_set_cmd(uint8_t chan, uint8_t mode, uint8_t speed)
{
uint16_t s = ((float)speed / 255) * 1000;
if (mode == 2) { // fw
motor_set_dir(chan, MOTOR_FW);
motor_set_speed(MOTOR_1, s);
} else if (mode == 1) { // bw
motor_set_dir(chan, MOTOR_BW);
motor_set_speed(MOTOR_1, s);
} else if (mode == 3) { // brake
motor_set_dir(chan, MOTOR_BRAKE);
motor_stop(MOTOR_1);
}
}
void set_output(state st) {
motor_set_dir(st.speed < 0);
motor_set_speed(((uint16_t)abs(st.speed)) >> 8);
led_set_brightness(st.light >> 8);
}
int main (void) {
uint8_t bu, bd; // buttons up, buttons down (read-in buffers)
uint8_t i; // iterator
uint8_t adcchan = 0;
uint8_t ramp;
uint16_t cycle;
motor_t motor[NMOTORS];
for (i = 0; i < NMOTORS; i++) {
motor[i].isrunning = false;
motor[i].position = 0;
motor[i].bu = false;
motor[i].bd = false;
motor[i].pot = 0;
motor[i].trgspeed = SPEEDMIN;
motor[i].curspeed = SPEEDMIN;
motor[i].counter = 0;
}
led_init();
spi_init();
adc_init();
motors_init();
/* // debug */
/* uart_init(); */
/* stdout = &uart_output; */
adc_set_chan(adcchan);
adc_start();
while (1) {
led_off();
// keep the ADC running "in background"
if ( !( adc_is_running() )) {
motor[adcchan].pot = adc_get();
if (motor[adcchan].bu != motor[adcchan].bd) {
motor[adcchan].trgspeed =
motor_scale_speed(motor[adcchan].pot);
} else {
motor[adcchan].trgspeed = SPEEDMIN;
}
adcchan++;
if (adcchan >= 5) adcchan = 0;
// HACK: lame jumpers
if (adcchan == 0) adc_set_chan(6);
if (adcchan == 1) adc_set_chan(1);
if (adcchan == 2) adc_set_chan(2);
if (adcchan == 3) adc_set_chan(3);
if (adcchan == 4) adc_set_chan(7);
adc_start();
}
// read in buttons
shiftreg_load();
bu = spi_transmit(SPI_TRANSMIT_DUMMY);
bd = spi_transmit(SPI_TRANSMIT_DUMMY);
for (i = 0; i < NMOTORS; i++) {
motor[i].bu = bit_is_set(bu, i) ? true : false;
motor[i].bd = bit_is_set(bd, i) ? true : false;
// both buttons pressed
if (motor[i].bu && motor[i].bd) {
led_on();
motor[i].trgspeed = SPEEDMIN;
}
if (motor[i].bu != motor[i].bd) {
motor[i].isrunning = true;
// set dir
if (motor[i].bu) {
motor_set_dir(i, DIR_UP);
} else if (motor[i].bd) {
motor_set_dir(i, DIR_DOWN);
}
}
// allow ramp-down
else if (motor[i].curspeed != motor[i].trgspeed) {
motor[i].isrunning = true;
}
// mark as ok-to-stop
else if (motor[i].curspeed == SPEEDMIN) {
motor[i].isrunning = false;
}
}
for (ramp = 0; ramp < RAMPCYCLES; ramp++) {
// push current speed towards target
for (i = 0; i < NMOTORS; i++) {
if (motor[i].curspeed < motor[i].trgspeed) {
motor[i].curspeed += 1;
}
if (motor[i].curspeed > motor[i].trgspeed) {
motor[i].curspeed -= 1;
}
}
// run at current speed
for (cycle = 0; cycle < RUNCYCLES; cycle++) {
for (i = 0; i < NMOTORS; i++) {
// either button pressed or ramp-down
if (motor[i].isrunning) {
if (motor[i].counter > 0) {
motor[i].counter -= 1;
} else {
// reset counter
motor[i].counter = motor[i].curspeed;
motor_step(i);
}
}
}
}
}
}
return 0;
}
void motor_full_bw(motor_chan_t motor)
{
motor_set_dir(motor, MOTOR_BW);
motor_set_speed(motor, MOTOR_SPEED_MAX);
}