static void handle_twi_command(void)
{
uint8_t command;
// Get the command from the receive buffer.
command = twi_receive_byte();
switch (command)
{
case TWI_CMD_RESET:
// Reset the servo.
watchdog_hard_reset();
break;
case TWI_CMD_PWM_ENABLE:
// Enable PWM to the servo motor.
pwm_enable();
break;
case TWI_CMD_PWM_DISABLE:
// Disable PWM to the servo motor.
pwm_disable();
break;
case TWI_CMD_WRITE_ENABLE:
// Enable write to read/write protected registers.
registers_write_enable();
break;
case TWI_CMD_WRITE_DISABLE:
// Disable write to read/write protected registers.
registers_write_disable();
break;
case TWI_CMD_REGISTERS_SAVE:
// Save register values into EEPROM.
eeprom_save_registers();
break;
case TWI_CMD_REGISTERS_RESTORE:
// Restore register values into EEPROM.
eeprom_restore_registers();
break;
case TWI_CMD_REGISTERS_DEFAULT:
// Restore register values to factory defaults.
registers_defaults();
break;
case TWI_CMD_EEPROM_ERASE:
// Erase the EEPROM.
eeprom_erase();
break;
case TWI_CMD_VOLTAGE_READ:
// Request a voltage reading.
adc_read_voltage();
break;
#if CURVE_MOTION_ENABLED
case TWI_CMD_CURVE_MOTION_ENABLE:
// Enable curve motion handling.
motion_enable();
break;
case TWI_CMD_CURVE_MOTION_DISABLE:
// Disable curve motion handling.
motion_disable();
break;
case TWI_CMD_CURVE_MOTION_RESET:
// Reset the motion to the current position.
#if ENCODER_ENABLED
motion_reset(enc_get_position_value());
#else
motion_reset(adc_get_position_value());
#endif
break;
case TWI_CMD_CURVE_MOTION_APPEND:
// Append motion curve data stored in the registers.
motion_append();
break;
#endif
default:
// Ignore unknown command.
break;
}
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
// Initialize the PWM module.
pwm_init();
// Initialize the ADC module.
adc_init();
// Initialize the PID algorithm module.
pid_init();
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
#if ENCODER_ENABLED
// Initialize software I2C to talk with encoder.
swi2c_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Wait until initial position value is ready.
{
int16_t position;
// Get start-up position
#if ENCODER_ENABLED
position=(int16_t) enc_get_position_value();
#else
while (!adc_position_value_is_ready());
position=(int16_t) adc_get_position_value();
#endif
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(position);
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, position);
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
}
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
pwm_enable();
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
uint8_t tick;
int16_t pwm;
int16_t position;
// Is ADC position value ready?
// NOTE: Even when the encoder is enabled, we still need the ADC potmeasurement as the
// clock because that is how the original firmware was written.
tick=adc_position_value_is_ready();
if(tick)
{
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
}
// Get the new position value.
if(tick)
{
position = (int16_t) adc_get_position_value(); // NOTE: For encoder builds, this is the clock: clear the flag
#if ENCODER_ENABLED
} // Always run the encoder (faster PID to PWM loop = better?)
position = (int16_t) enc_get_position_value();
if (position >= 0)
{
#endif
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position, tick);
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
}
// Is a power value ready?
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
static void handle_twi_command(void)
{
uint8_t command;
// Get the command from the receive buffer.
command = twi_receive_byte();
switch (command)
{
case TWI_CMD_RESET:
// Reset the servo.
watchdog_hard_reset();
break;
case TWI_CMD_PWM_ENABLE:
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Enable PWM to the servo motor.
pwm_enable();
#endif
#if STEP_ENABLED
// Enable Stepper motor control.
step_enable();
#endif
break;
case TWI_CMD_PWM_DISABLE:
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Disable PWM to the servo motor.
pwm_disable();
#endif
#if STEP_ENABLED
// Enable Stepper motor control.
step_disable();
#endif
break;
case TWI_CMD_WRITE_ENABLE:
// Enable write to read/write protected registers.
registers_write_enable();
break;
case TWI_CMD_WRITE_DISABLE:
// Disable write to read/write protected registers.
registers_write_disable();
break;
case TWI_CMD_REGISTERS_SAVE:
// Save register values into EEPROM.
eeprom_save_registers();
break;
case TWI_CMD_REGISTERS_RESTORE:
// Restore register values into EEPROM.
eeprom_restore_registers();
break;
case TWI_CMD_REGISTERS_DEFAULT:
// Restore register values to factory defaults.
registers_defaults();
break;
case TWI_CMD_EEPROM_ERASE:
// Erase the EEPROM.
eeprom_erase();
break;
#if CURVE_MOTION_ENABLED
case TWI_CMD_CURVE_MOTION_ENABLE:
// Enable curve motion handling.
motion_enable();
break;
case TWI_CMD_CURVE_MOTION_DISABLE:
// Disable curve motion handling.
motion_disable();
break;
case TWI_CMD_CURVE_MOTION_RESET:
// Reset the motion to the current position.
motion_reset(adc_get_position_value());
break;
case TWI_CMD_CURVE_MOTION_APPEND:
// Append motion curve data stored in the registers.
motion_append();
break;
#endif
case TWI_CMD_GCALL_ENABLE:
// Enable the general call functionality
general_call_enable();
break;
case TWI_CMD_GCALL_DISABLE:
// Disable General call functionaility
general_call_disable();
break;
case TWI_CMD_GCALL_START_MOVE:
// start the general call movement
general_call_start_move();
break;
case TWI_CMD_GCALL_START_WAIT:
// dont move unless we get the start command or the group command
general_call_start_wait();
break;
case TWI_CMD_PWM_BRAKE_ENABLE:
#if PWM_ENH_ENABLED
pwm_brake_enable();
#endif
break;
case TWI_CMD_PWM_BRAKE_DISABLE:
#if PWM_ENH_ENABLED
pwm_brake_disable();
#endif
break;
default:
// Ignore unknown command.
break;
}
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
// Initialize the PWM module.
pwm_init();
// Initialize the ADC module.
adc_init();
#if ESTIMATOR_ENABLED
// Initialize the state estimator module.
estimator_init();
#endif
#if REGULATOR_MOTION_ENABLED
// Initialize the regulator algorithm module.
regulator_init();
#endif
#if PID_MOTION_ENABLED
// Initialize the PID algorithm module.
pid_init();
#endif
#if IPD_MOTION_ENABLED
// Initialize the IPD algorithm module.
ipd_init();
#endif
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Wait until initial position value is ready.
while (!adc_position_value_is_ready());
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(adc_get_position_value());
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
pwm_enable();
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
// Is position value ready?
if (adc_position_value_is_ready())
{
int16_t pwm;
int16_t position;
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
// Get the new position value.
position = (int16_t) adc_get_position_value();
#if ESTIMATOR_ENABLED
// Estimate velocity.
estimate_velocity(position);
#endif
#if PID_MOTION_ENABLED
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position);
#endif
#if IPD_MOTION_ENABLED
// Call the IPD algorithm module to get a new PWM value.
pwm = ipd_position_to_pwm(position);
#endif
#if REGULATOR_MOTION_ENABLED
// Call the state regulator algorithm module to get a new PWM value.
pwm = regulator_position_to_pwm(position);
#endif
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
}
// Is a power value ready?
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Initialize the PWM module.
pwm_init();
#endif
#if STEP_ENABLED
// Initialise the stepper motor
step_init();
#endif
// Initialize the ADC module.
adc_init();
// Initialise the Heartbeart
heartbeat_init();
// Initialize the PID algorithm module.
pid_init();
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
#if BACKEMF_ENABLED
// Initialise the back emf module
backemf_init();
#endif
#if ALERT_ENABLED
//initialise the alert registers
alert_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(banks_read_byte(POS_PID_BANK, REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Trigger the adc sampling hardware
adc_start(ADC_CHANNEL_POSITION);
// Wait until initial position value is ready.
while (!adc_position_value_is_ready());
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(adc_get_position_value());
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
pwm_enable();
#endif
#if STEP_ENABLED
step_enable();
#endif
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
static uint8_t emf_motor_is_coasting = 0;
// Is the system heartbeat ready?
if (heartbeat_is_ready())
{
static int16_t last_seek_position;
static int16_t wait_seek_position;
static int16_t new_seek_position;
// Clear the heartbeat flag
heartbeat_value_clear_ready();
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
// General call support
// Check to see if we have the wait flag enabled. If so save the new position, and write in the
// old position until we get the move command
if (general_call_enabled())
{
//we need to wait for the go command before moving
if (general_call_wait())
{
// store the new position, but let the servo lock to the last seek position
wait_seek_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
if (wait_seek_position != last_seek_position) // do we have a new position?
{
new_seek_position = wait_seek_position;
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, last_seek_position);
}
}
last_seek_position = registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
//check to make sure that we can start the move.
if (general_call_start() ||
( registers_read_byte(REG_GENERAL_CALL_GROUP_START) == banks_read_byte(CONFIG_BANK, REG_GENERAL_CALL_GROUP)))
{
// write the new position with the previously saved position
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, new_seek_position);
general_call_start_wait_reset(); // reset the wait flag
general_call_start_reset(); // reset the start flag
}
}
#if BACKEMF_ENABLED
// Quick and dirty check to see if pwm is active. This is done to make sure the motor doesn't
// whine in the audible range while idling.
uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
if (pwm_a || pwm_b)
{
// Disable PWM
backemf_coast_motor();
emf_motor_is_coasting = 1;
}
else
{
// reset the back EMF value to 0
banks_write_word(INFORMATION_BANK, REG_BACKEMF_HI, REG_BACKEMF_LO, 0);
emf_motor_is_coasting = 0;
}
#endif
#if ADC_ENABLED
// Trigger the adc sampling hardware. This triggers the position and temperature sample
adc_start(ADC_FIRST_CHANNEL);
#endif
}
// Wait for the samples to complete
#if TEMPERATURE_ENABLED
if (adc_temperature_value_is_ready())
{
// Save temperature value to registers
registers_write_word(REG_TEMPERATURE_HI, REG_TEMPERATURE_LO, (uint16_t)adc_get_temperature_value());
}
#endif
#if CURRENT_ENABLED
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
#endif
#if ADC_POSITION_ENABLED
if (adc_position_value_is_ready())
{
int16_t position;
// Get the new position value from the ADC module.
position = (int16_t) adc_get_position_value();
#else
if (position_value_is_ready())
{
int16_t position;
// Get the position value from an external module.
position = (int16_t) get_position_value();
#endif
int16_t pwm;
#if BACKEMF_ENABLED
if (emf_motor_is_coasting == 1)
{
uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
// Quick and dirty check to see if pwm is active
if (pwm_a || pwm_b)
{
// Get the backemf sample.
backemf_get_sample();
// Turn the motor back on
backemf_restore_motor();
emf_motor_is_coasting = 0;
}
}
#endif
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position);
#if ALERT_ENABLED
// Update the alert status registers and do any throttling
alert_check();
#endif
// Allow any alerts to modify the PWM value.
pwm = alert_pwm_throttle(pwm);
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
#endif
#if STEP_ENABLED
// Update the stepper motor as indicated by the PWM value.
// Sanity checks are performed against the position value.
step_update(position, pwm);
#endif
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
// Update the bank register operations
banks_update_registers();
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
