void motion_registers_reset(void)
// Reset the motion registers to zero values.
{
// Set the default position, velocity and delta data.
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0);
registers_write_word(REG_CURVE_IN_VELOCITY_HI, REG_CURVE_IN_VELOCITY_LO, 0);
registers_write_word(REG_CURVE_OUT_VELOCITY_HI, REG_CURVE_OUT_VELOCITY_LO, 0);
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 0);
// Update the buffer status.
registers_write_byte(REG_CURVE_RESERVED, 0);
registers_write_byte(REG_CURVE_BUFFER, motion_buffer_left());
}
void regulator_registers_defaults(void)
// Initialize the motion related register values. This is done here to
// keep the motion related code in a single file.
{
// Default control parameters.
registers_write_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO, 510); // k1
registers_write_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO, 23287); // k2
// Default position limits.
registers_write_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO, 0x0060);
registers_write_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO, 0x03A0);
// Default reverse seek setting.
registers_write_byte(REG_REVERSE_SEEK, 0x00);
}
void ipd_registers_defaults(void)
// Initialize the PID algorithm related register values. This is done
// here to keep the PID related code in a single file.
{
// Default gain values.
registers_write_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO, 0x0400);
registers_write_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO, 0x0300);
registers_write_word(REG_PID_IGAIN_HI, REG_PID_IGAIN_LO, 0x4000);
// Default position limits.
registers_write_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO, 0x0060);
registers_write_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO, 0x03A0);
// Default reverse seek setting.
registers_write_byte(REG_REVERSE_SEEK, 0x00);
}
void pwm_registers_defaults(void)
// Initialize the PWM algorithm related register values. This is done
// here to keep the PWM related code in a single file.
{
// PWM divider is a value between 1 and 1024. This divides the fundamental
// PWM frequency (500 kHz for 8MHz clock, 1250 kHz for 20MHz clock)(div=16) by a
// constant value to produce a PWM frequency suitable to drive a motor. A
// small motor with low inductance and impedance such as those found in an
// RC servo will my typically use a divider value between 16 and 64. A larger
// motor with higher inductance and impedance may require a greater divider.
registers_write_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO, DEFAULT_PWM_FREQ_DIVIDER);
}
void motion_next(uint16_t delta)
// Increment the buffer counter by the indicated delta and return the position
// and velocity from the buffered curves. If the delta is zero the current
// position and velocity is returned.
{
float fposition;
float fvelocity;
// Determine if curve motion is disabled in the registers.
if (!(registers_read_byte(REG_FLAGS_LO) & (1<<FLAGS_LO_MOTION_ENABLED))) return;
// Are we processing an empty curve?
if (motion_tail == motion_head)
{
// Yes. Keep the counter and duration at zero.
motion_counter = 0;
motion_duration = 0;
}
else
{
// Increment the counter.
motion_counter += delta;
// Have we exceeded the duration of the currently buffered curve?
while (motion_counter > curve_get_duration())
{
// Reduce the buffer counter by the currently buffered curve duration.
motion_counter -= curve_get_duration();
// Reduce the buffer duration by the currently buffered curve duration.
motion_duration -= curve_get_duration();
// Increment the tail to process the next buffered curve.
motion_tail = (motion_tail + 1) & MOTION_BUFFER_MASK;
// Has the tail caught up with the head?
if (motion_tail == motion_head)
{
// Initialize an empty hermite curve with a zero duration. This is a degenerate case for
// the hermite cuve that will always return the position of the curve without velocity.
curve_init(0, 0, keys[motion_head].position, keys[motion_head].position, 0.0, 0.0);
// Reset the buffer counter and duration to zero.
motion_counter = 0;
motion_duration = 0;
}
else
{
uint8_t curr_point;
uint8_t next_point;
// Get the current point and next point for the curve.
curr_point = motion_tail;
next_point = (curr_point + 1) & MOTION_BUFFER_MASK;
// Initialize the hermite curve from the current and next point.
curve_init(0, keys[next_point].delta,
keys[curr_point].position, keys[next_point].position,
keys[curr_point].out_velocity, keys[next_point].in_velocity);
}
// Update the space available in the buffer.
registers_write_byte(REG_CURVE_BUFFER, motion_buffer_left());
}
}
// Get the position and velocity from the hermite curve.
curve_solve(motion_counter, &fposition, &fvelocity);
// The velocity is in position units a millisecond, but we really need the
// velocity to be measured in position units every 10 milliseconds to match
// the sample period of the ADC.
fvelocity *= 10.0;
// Update the seek position register.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, float_to_int(fposition));
// Update the seek velocity register.
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, float_to_int(fvelocity));
}
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;
}
int16_t regulator_position_to_pwm(int16_t current_position)
// This function takes the current servo position as input and outputs a pwm
// value for the servo motors. The current position value must be within the
// range 0 and 1023. The output will be within the range of -255 and 255 with
// values less than zero indicating clockwise rotation and values more than
// zero indicating counter-clockwise rotation.
{
int16_t k1;
int16_t k2;
int16_t output;
int16_t command_position;
int16_t current_velocity;
int16_t current_error;
// Get the command position to where the servo is moving to from the registers.
command_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
// Get estimated velocity
current_velocity = (int16_t) registers_read_word(REG_VELOCITY_HI, REG_VELOCITY_LO);
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Update the system registers with an adjusted reverse sense
// position. With reverse sense, the position value to grows from
// a low value to high value in the clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) (MAX_POSITION - current_position));
// Adjust command position for the reverse sense.
command_position = MAX_POSITION - command_position;
}
else
{
// No. Update the system registers with a non-reverse sense position.
// Normal position value grows from a low value to high value in the
// counter-clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) current_position);
}
// Get the control parameters.
k1 = (int16_t) registers_read_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO);
k2 = (int16_t) registers_read_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO);
// Determine the current error.
current_error = command_position - current_position;
// The following operations are fixed point operations. To add/substract
// two fixed point values they must have the same fractional precision
// (the same number of bits behind the decimal). When two fixed point
// values are multiplied the fractional precision of the result is the sum
// of the fractional precision of the the the factors (the sum of the bits
// behind the decimal of each factor). To reach the best possible precision
// the fixed point bit is chosen for each variable separately according to
// its maximum and dimension. A shift factor is then applied after
// multiplication in the fixed_multiply() function to adjust the fractional
// precision of the product for addition or subtraction.
// Used fixed point bits, counted from the lowest bit:
// Control Param. k1: fp_k1 = 5
// Control Param. k2: fp_k2 = 5
// Position state z1: fp_z1 = 5
// Velocity state z2: fp_z2 = 11
// Real Position x1: fp_x1 = 0
// PWM output: fp_output = 0
// output = k1 * x1 + k2 * x2
output = fixed_multiply(k1, current_error, 5); // fp: 5 + 0 -> 0 : rshift = 5
output += fixed_multiply(k2, -current_velocity, 16); // fp: 5 + 11 -> 0 : rshift = 16
// Check for output saturation.
if (output > MAX_OUTPUT) output = MAX_OUTPUT;
if (output < MIN_OUTPUT) output = MIN_OUTPUT;
return output;
}
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;
}
int16_t ipd_position_to_pwm(int16_t current_position)
// This function takes the current servo position as input and outputs a pwm
// value for the servo motors. The current position value must be within the
// range 0 and 1023. The output will be within the range of -255 and 255 with
// values less than zero indicating clockwise rotation and values more than
// zero indicating counter-clockwise rotation.
//
// The feedback approach implemented here was first published in Richard Phelan's
// Automatic Control Systems, Cornell University Press, 1977 (ISBN 0-8014-1033-9)
//
// The theory of operation of this function will be filled in later, but the
// diagram below should gives a general picture of how it is intended to work.
//
//
// +<------- bounds checking -------+
// | |
// |¯¯¯¯¯| |¯¯¯¯¯¯¯¯| |¯¯¯¯¯| |¯¯¯¯¯¯¯¯¯| |
// command -->| - |-->|integral|-->| - |-->| motor |-->+-> actuator
// |_____| |________| |_____| |_________| |
// | | |
// | +<-- Kv * velocity --+
// | | |
// | +<-- Kp * position --+
// | |
// +<-------------Ki * position ---------------+
//
//
{
int16_t deadband;
int16_t command_position;
int16_t maximum_position;
int16_t minimum_position;
int16_t current_velocity;
int16_t command_error;
int16_t output;
int16_t position_output;
int16_t velocity_output;
int16_t integral_output;
uint16_t position_gain;
uint16_t velocity_gain;
uint16_t integral_gain;
// Determine the velocity as the difference between the current and previous position.
current_velocity = current_position - previous_position;
// Update the previous position.
previous_position = current_position;
// Get the command position to where the servo is moving to from the registers.
command_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
minimum_position = (int16_t) registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
maximum_position = (int16_t) registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
// Set the deadband value and divide by two for calculations below.
deadband = 2;
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Update the system registers with an adjusted reverse sense
// position. With reverse sense, the position value to grows from
// a low value to high value in the clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) (MAX_POSITION - current_position));
// Adjust command position for the reverse sense.
command_position = MAX_POSITION - command_position;
minimum_position = MAX_POSITION - minimum_position;
maximum_position = MAX_POSITION - maximum_position;
}
else
{
// No. Update the system registers with a non-reverse sense position.
// Normal position value grows from a low value to high value in the
// counter-clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) current_position);
}
// Sanity check the command position. We do this because this value is
// passed from the outside to the servo and it could be an invalid value.
if (command_position < minimum_position) command_position = minimum_position;
if (command_position > maximum_position) command_position = maximum_position;
// The command error is the difference between the command position and current position.
command_error = command_position - current_position;
// Adjust proportional error due to deadband. The potentiometer readings are a
// bit noisy and there is typically one or two units of difference from reading
// to reading when the servo is holding position. Adding deadband decreases some
// of the twitchiness in the servo caused by this noise.
if (command_error > deadband)
{
// Factor out deadband from the command error.
command_error -= deadband;
}
else if (command_error < -deadband)
{
// Factor out deadband from the command error.
command_error += deadband;
}
else
{
// Adjust to command error to zero within deadband.
command_error = 0;
}
// Get the positional, velocity and integral gains from the registers.
position_gain = registers_read_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO);
velocity_gain = registers_read_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO);
integral_gain = registers_read_word(REG_PID_IGAIN_HI, REG_PID_IGAIN_LO);
// Add the command error scaled by the position gain to the integral accumulator.
// The integral accumulator maintains a sum of total error over each iteration.
integral_accumulator_update(command_error, integral_gain);
// Get the integral output. There is no gain applied to the integral output.
integral_output = integral_accumulator_get();
// Determine the position output component. We multiply the position gain
// by the current position of the servo to create the position output.
position_output = fixed_multiply(current_position, position_gain);
// Determine the velocity output component. We multiply the velocity gain
// by the current velocity of the servo to create the velocity output.
velocity_output = fixed_multiply(current_velocity, velocity_gain);
// registers_write_word(REG_RESERVED_18, REG_RESERVED_19, (uint16_t) position_output);
// registers_write_word(REG_RESERVED_1A, REG_RESERVED_1B, (uint16_t) velocity_output);
// registers_write_word(REG_RESERVED_1C, REG_RESERVED_1D, (uint16_t) integral_output);
// The integral output drives the output and the position and velocity outputs
// function as a frictional component to counter the integral output.
output = integral_output - position_output - velocity_output;
// Is the output saturated? If so we need limit the output and clip
// the integral accumulator just at the saturation level.
if (output < MIN_OUTPUT)
{
// Calculate a new integral accumulator based on the output range. This value
// is calculated to keep the integral output just on the verge of saturation.
integral_accumulator_reset(position_output + velocity_output + MIN_OUTPUT);
// Limit the output.
output = MIN_OUTPUT;
}
else if (output > MAX_OUTPUT)
{
// Calculate a new integral accumulator based on the output range. This value
// is calculated to keep the integral output just on the verge of saturation.
integral_accumulator_reset(position_output + velocity_output + MAX_OUTPUT);
// Limit the output.
output = MAX_OUTPUT;
}
// Return the output.
return output;
}
void pulse_control_update(void)
// Update the servo seek position and velocity based on any received servo pulses.
{
int16_t pulse_position;
// Did we get a pulse?
if (pulse_flag)
{
// Ignore unusual pulse durations as a sanity check.
if ((pulse_duration > 500) && (pulse_duration < 2500))
{
// Convert the pulse duration to a pulse time.
pulse_position = pulse_duration - 998;
// Limit the pulse position.
if (pulse_position < MIN_POSITION) pulse_position = MIN_POSITION;
if (pulse_position > MAX_POSITION) pulse_position = MAX_POSITION;
// Apply a low pass filter to the pulse position.
pulse_position = filter_update(pulse_position, &filter_reg_pulse);
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Update the seek position using reverse sense.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, (MAX_POSITION - pulse_position));
}
else
{
// No. Update the seek position using normal sense.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, pulse_position);
}
// The seek velocity will always be zero.
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// Make sure pwm is enabled.
pwm_enable();
// Update the pulse time used as a time stamp.
pulse_time = timer_get();
}
// Reset the pulse time flag.
pulse_flag = 0;
}
else
{
// If we haven't seen a pulse in .5 seconds disable pwm.
if (timer_delta(pulse_time) > 50)
{
// Disable pwm.
pwm_disable();
// Update the pulse time used as a time stamp.
pulse_time = timer_get();
}
}
return;
}
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;
}
int16_t speed_position_to_pwm(int16_t current_position)
// This is a modified pi algorithm with feed forward by which the seek
// veolcity is assumed to be a moving target. The algorithm attempts to
// output a pwm value that will achieve a predicted velocity.
{
// We declare these static to keep them off the stack.
static int16_t p_component;
static int16_t i_component;
static int16_t seek_velocity;
static int16_t current_velocity;
static int32_t pwm_output;
static uint16_t i_gain;
static uint16_t p_gain;
static int16_t anti_windup;
static uint16_t feed_forward_gain;
static int16_t max_speed;
// Get the seek velocity.
seek_velocity = (int16_t) registers_read_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO);
max_speed = (int16_t)banks_read_word(POS_PID_BANK, REG_MAX_SPEED_HI, REG_MAX_SPEED_LO);
if (seek_velocity> max_speed) seek_velocity = max_speed;
if (seek_velocity<-max_speed) seek_velocity = -max_speed;
// Get current velocity from measurement of back-emf
current_velocity = banks_read_word(INFORMATION_BANK, REG_BACKEMF_HI, REG_BACKEMF_LO);
// Determine rotation sense by last PWM value
if (registers_read_byte(REG_PWM_DIRA)) current_velocity = - current_velocity;
// Are we reversing the seek sense?
if (banks_read_byte(POS_PID_BANK, REG_REVERSE_SEEK) != 0)
{
registers_write_word(REG_VELOCITY_HI, REG_VELOCITY_LO, (uint16_t) -current_velocity);
}
else
{
// No. Update the position and velocity registers without change.
registers_write_word(REG_VELOCITY_HI, REG_VELOCITY_LO, (uint16_t) current_velocity);
}
// Get the proportional and integral gains.
p_gain = banks_read_word(POS_PID_BANK, REG_PID_PGAIN_HI, REG_PID_PGAIN_LO);
i_gain = banks_read_word(POS_PID_BANK, REG_PID_IGAIN_HI, REG_PID_IGAIN_LO);
feed_forward_gain = banks_read_word(POS_PID_BANK, REG_FEED_FORWARD_HI, REG_FEED_FORWARD_LO);
anti_windup = (int16_t)banks_read_word(POS_PID_BANK, REG_ANTI_WINDUP_HI, REG_ANTI_WINDUP_LO);
// The proportional component to the PID is the position error.
p_component = seek_velocity - current_velocity;
// Increment the integral component of the PID
i_component += (int32_t) p_component * (int32_t) i_gain;
// Saturate the integrator for anti wind-up
if (i_component > anti_windup) i_component = anti_windup;
if (i_component < -anti_windup) i_component = -anti_windup;
// Start with zero PWM output.
pwm_output = 0;
// Apply the feed forward component of the PWM output.
pwm_output += (int32_t) seek_velocity * (int32_t) feed_forward_gain;
// Apply the proportional component of the PWM output.
pwm_output += (int32_t) p_component * (int32_t) p_gain;
// Apply the integral component of the PWM output.
pwm_output += i_component;
// Shift by 8 to account for the multiply by the 8:8 fixed point gain values.
pwm_output >>= 8;
// Check for output saturation.
if (pwm_output > MAX_OUTPUT)
{
// Can't go higher than the maximum output value.
pwm_output = MAX_OUTPUT;
}
else if (pwm_output < MIN_OUTPUT)
{
// Can't go lower than the minimum output value.
pwm_output = MIN_OUTPUT;
}
// Return the PID output.
return (int16_t) pwm_output;
//return (int16_t) registers_read_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO);
}
