void handle_motor_pot (byte target_vol) { // a state-driven dispatcher
static int motor_stabilized;
int sensed_value;
int target_value = l_map(target_vol, VOL_MIN, VOL_MAX, POT_MIN, POT_MAX);
switch (pot_state) {
case MOTOR_INIT: // initial state, just go to 'settled' from here
pot_state = MOTOR_SETTLED;
vol_last_seen = target_vol;
break;
case MOTOR_SETTLED:
if (target_vol != vol_last_seen) { // If volume has changed since last time then
pot_last_seen = pot_read_smoothed(); // save current value of pot and
pot_state = MOTOR_IN_MOTION; // say we're in motion next time around.
//sensed_value = pot_read_smoothed();
}
vol_last_seen = target_vol;
break;
case MOTOR_IN_MOTION:
lcd.restore_backlight();
motor_stabilized = 0;
sensed_value = pot_read_smoothed();
if ( abs(target_value - sensed_value) <= 4 ) {
motor_drive(LOW, LOW); // Stop
pot_state = MOTOR_COASTING;
delay(5); // 5ms
return;
} else { // not at target volume yet
if (sensed_value < target_value ) {
motor_drive(HIGH, LOW); // Go CW
} else if (sensed_value > target_value ) {
motor_drive(LOW, HIGH); // Go CCW
}
}
break;
case MOTOR_COASTING:
// we are waiting for the motor to stop (which means the last value == this value)
lcd.restore_backlight();
delay(20);
sensed_value = pot_read_smoothed();
if (sensed_value == pot_last_seen) {
if (++motor_stabilized >= 5) {
pot_state = MOTOR_SETTLED; // yay! we reached our target
}
} else {
motor_stabilized = 0; // we found a value that didn't match, so reset our 'sameness' counter
}
pot_last_seen = sensed_value; // this is the oper value of the pot, from the ADC
break;
default:
;
}
}
void position_controller( char pos )
{
int16_t rotations = encoder_read();
int16_t prefered_rotations = ref_pos + ( ( (int32_t)pos ) * 8000.0) / 255;
int16_t error = -prefered_rotations + rotations;
char to_motor = error >> 6;
motor_drive(to_motor*2);
}
void motor_init(void)
{
i2c_init();
DDRF = 0xFF; // MJ1 output
DDRK = 0x00;
PORTF |= (1 << PF4); // Enable motor
//Move to known reference point
motor_drive(-50);
_delay_ms(1000);
motor_drive(0);
//Toggle reset encoder
PORTF &= ~(1 << PF6);
_delay_us(5);
PORTF |= (1 << PF6);
ref_pos = encoder_read();
}
/*
* Main Loop
*/
int main()
{
motor_init();
button_init();
log_init();
g_tick = 0;
LOG(LVL_INFO, "------------------------");
LOG(LVL_INFO, "Starting application");
while(1)
{
g_tick++; /* note: will roll over */
motor_serviceEncoders();
motor_drive();
button_readState();
print_service();
log_service();
}
}
