void alert_check(void)
// Check the alert registers for min/max overflows and set the status register accordingly
{
uint16_t voltage;
uint16_t current;
uint16_t temperature;
uint16_t max_voltage;
uint16_t min_voltage;
uint16_t max_current;
uint16_t max_temperature;
// Get the current voltage and power
voltage = registers_read_word(REG_VOLTAGE_HI,REG_VOLTAGE_LO);
current = registers_read_word(REG_POWER_HI,REG_POWER_LO);
temperature = registers_read_word(REG_TEMPERATURE_HI,REG_TEMPERATURE_LO);
// Get the set limits for voltage and power
max_voltage = banks_read_word(ALERT_CONFIG_BANK, ALERT_VOLT_MAX_LIMIT_HI, ALERT_VOLT_MAX_LIMIT_LO);
min_voltage = banks_read_word(ALERT_CONFIG_BANK, ALERT_VOLT_MIN_LIMIT_HI, ALERT_VOLT_MIN_LIMIT_LO);
max_temperature = banks_read_word(ALERT_CONFIG_BANK, ALERT_TEMP_MAX_LIMIT_HI, ALERT_TEMP_MAX_LIMIT_LO);
max_current = banks_read_word(ALERT_CONFIG_BANK, ALERT_CURR_MAX_LIMIT_HI, ALERT_CURR_MAX_LIMIT_LO);
// Check the voltage is not below or above the set voltage. Ignore if 0
// NOTE: This would be a good place to alter the pwm of the motor to output the same voltage
if (voltage > max_voltage && max_voltage >0)
{
alert_setbit(ALERT_OVERVOLT);
}
else if (voltage < min_voltage && min_voltage >0)
{
alert_setbit(ALERT_UNDERVOLT);
}
// Check the curent is not over the maximum set current. Ignore if 0
// NOTE: This would be a good place to throttle the current if we want to
if (current > max_current && max_current >0)
{
alert_setbit(ALERT_OVERCURR);
throttle = current - max_current ;
}
// Check the curent is not over the maximum set current. Ignore if 0
// NOTE: This would be a good place to throttle the current if we want to
if (temperature > max_temperature && max_temperature >0)
{
alert_setbit(ALERT_OVERTEMP);
// Turn off pwm?
pwm_disable();
}
if(throttle >0) throttle--;
}
uint8_t motion_append(void)
// Append a new curve keypoint from data stored in the curve registers. The keypoint
// is offset from the previous curve by the specified delta. An error is returned if
// there is no more room to store the new keypoint in the buffer or if the delta is
// less than one (a zero delta is not allowed).
{
int16_t position;
int16_t in_velocity;
int16_t out_velocity;
uint8_t next;
uint16_t delta;
// Get the next index in the buffer.
next = (motion_head + 1) & MOTION_BUFFER_MASK;
// Return error if we have looped the head to the tail and the buffer is filled.
if (next == motion_tail) return 0;
// Get the position, velocity and time delta values from the registers.
position = (int16_t) registers_read_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO);
in_velocity = (int16_t) registers_read_word(REG_CURVE_IN_VELOCITY_HI, REG_CURVE_IN_VELOCITY_LO);
out_velocity = (int16_t) registers_read_word(REG_CURVE_OUT_VELOCITY_HI, REG_CURVE_OUT_VELOCITY_LO);
delta = (uint16_t) registers_read_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO);
// Keypoint delta must be greater than zero.
if (delta < 1) return 0;
// Fill in the next keypoint.
keys[next].delta = delta;
keys[next].position = int_to_float(position);
keys[next].in_velocity = fixed_to_float(in_velocity);
keys[next].out_velocity = fixed_to_float(out_velocity);
// Is this keypoint being added to an empty buffer?
if (motion_tail == motion_head)
{
// Initialize a new hermite curve that gets us from the current position to the new position.
// We use a velocity of zero at each end to smoothly transition from one to the other.
curve_init(0, delta, curve_get_p1(), keys[next].position, 0.0, 0.0);
}
// Increase the duration of the buffer.
motion_duration += delta;
// Set the new head index.
motion_head = next;
// Reset the motion registers and update the buffer status.
motion_registers_reset();
return 1;
}
void pwm_init(void)
// Initialize the PWM module for controlling a DC motor.
{
// Initialize the pwm frequency divider value.
pwm_div = registers_read_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO);
// Set EN_A (PD2) and EN_B (PD3) to low.
PORTD &= ~((1<<PD2) | (1<<PD3));
// Set SMPLn_B (PD4) and SMPLn_A (PD7) to high.
PORTD |= ((1<<PD4) | (1<<PD7));
// Enable PD2, PD3, PD4 and PD7 as outputs.
DDRD |= ((1<<DDD2) | (1<<DDD3) | (1<<DDD4) | (1<<DDD7));
// Set PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) are low.
PORTB &= ~((1<<PB1) | (1<<PB2));
// Enable PB1/OC1A and PB2/OC1B as outputs.
DDRB |= ((1<<DDB1) | (1<<DDB2));
// Reset the timer1 configuration.
TCNT1 = 0; // Timer/Counter1 1,2,...X
TCCR1A = 0; // Timer/Counter1 Control Register A
TCCR1B = 0; // Timer/Counter1 Control Register B
TCCR1C = 0; // Timer/Counter1 Control Register C
TIMSK1 = 0; // Timer/Counter1 Interrupt Mask
// Set timer top value.
ICR1 = PWM_TOP_VALUE(pwm_div); //Input Capture Register1
// Set the PWM duty cycle to zero.
OCR1A = 0; //Output Compare Register1 A
OCR1B = 0; //Output Compare Register1 B
// Configure timer 1 for PWM, Phase and Frequency Correct operation, but leave outputs disabled.
TCCR1A = (0<<COM1A1) | (0<<COM1A0) | // Disable OC1A output.
(0<<COM1B1) | (0<<COM1B0) | // Disable OC1B output.
(0<<WGM11) | (0<<WGM10); // PWM, Phase and Frequency Correct, TOP = ICR1
TCCR1B = (0<<ICNC1) | (0<<ICES1) | // Input on ICP1 disabled.
(1<<WGM13) | (0<<WGM12) | // PWM, Phase and Frequency Correct, TOP = ICR1
(0<<CS12) | (0<<CS11) | (1<<CS10); // No prescaling.
// Update the pwm values.
registers_write_byte(REG_PWM_DIRA, 0);
registers_write_byte(REG_PWM_DIRB, 0);
}
void pwm_init(void)
// Initialize the PWM module for controlling a DC motor.
{
// Initialize the pwm frequency divider value.
pwm_div = registers_read_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO);
TCCR1A = 0;
asm("nop");
asm("nop");
asm("nop");
// Set PB1/OC1A and PB2/OC1B to low.
PORTB &= ~((1<<PB1) | (1<<PB2));
// Enable PB1/OC1A and PB2/OC1B as outputs.
DDRB |= ((1<<DDB1) | (1<<DDB2));
// Reset the timer1 configuration.
TCNT1 = 0;
TCCR1A = 0;
TCCR1B = 0;
TCCR1C = 0;
TIMSK1 = 0;
// Set timer top value.
ICR1 = PWM_TOP_VALUE(pwm_div);
// Set the PWM duty cycle to zero.
OCR1A = 0;
OCR1B = 0;
// Configure timer 1 for PWM, Phase and Frequency Correct operation, but leave outputs disabled.
TCCR1A = (0<<COM1A1) | (0<<COM1A0) | // Disable OC1A output.
(0<<COM1B1) | (0<<COM1B0) | // Disable OC1B output.
(0<<WGM11) | (0<<WGM10); // PWM, Phase and Frequency Correct, TOP = ICR1
TCCR1B = (0<<ICNC1) | (0<<ICES1) | // Input on ICP1 disabled.
(1<<WGM13) | (0<<WGM12) | // PWM, Phase and Frequency Correct, TOP = ICR1
(0<<CS12) | (0<<CS11) | (1<<CS10); // No prescaling.
// Update the pwm values.
registers_write_byte(REG_PWM_DIRA, 0);
registers_write_byte(REG_PWM_DIRB, 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;
}
void pwm_update(uint16_t position, int16_t pwm)
// Update the PWM signal being sent to the motor. The PWM value should be
// a signed integer in the range of -255 to -1 for clockwise movement,
// 1 to 255 for counter-clockwise movement or zero to stop all movement.
// This function provides a sanity check against the servo position and
// will prevent the servo from being driven past a minimum and maximum
// position.
{
uint8_t pwm_width;
uint16_t min_position;
uint16_t max_position;
// Quick check to see if the frequency divider changed. If so we need to
// configure a new top value for timer/counter1. This value should only
// change infrequently so we aren't too elegant in how we handle updating
// the value. However, we need to be careful that we don't configure the
// top to a value lower than the counter and compare values.
if (registers_read_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO) != pwm_div)
{
// Hold EN_A (PD2) and EN_B (PD3) low.
PORTD &= ~((1<<PD2) | (1<<PD3));
// Give the H-bridge time to respond to the above, failure to do so or to wait long
// enough will result in brownouts as the power is "crowbarred" to varying extents.
// The delay required is also dependant on factors which may affect the speed with
// which the MOSFETs can respond, such as the impedance of the motor, the supply
// voltage, etc.
//
// Experiments (with an "MG995") have shown that 5microseconds should be sufficient
// for most purposes.
//
delay_loop(DELAYLOOP);
// Disable OC1A and OC1B outputs.
TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0));
TCCR1A &= ~((1<<COM1B1) | (1<<COM1B0));
// Make sure that PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) are held low.
PORTB &= ~((1<<PB1) | (1<<PB2));
// Reset the A and B direction flags.
pwm_a = 0;
pwm_b = 0;
// Update the pwm frequency divider value.
pwm_div = registers_read_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO);
// Update the timer top value.
ICR1 = PWM_TOP_VALUE(pwm_div);
// Reset the counter and compare values to prevent problems with the new top value.
TCNT1 = 0;
OCR1A = 0;
OCR1B = 0;
}
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Swap the minimum and maximum position.
// Get the minimum and maximum seek position.
min_position = registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
max_position = registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
// Make sure these values are sane 10-bit values.
if (min_position > MAX_POSITION) min_position = MAX_POSITION;
if (max_position > MAX_POSITION) max_position = MAX_POSITION;
// Adjust the values because of the reverse sense.
min_position = MAX_POSITION - min_position;
max_position = MAX_POSITION - max_position;
}
else
{
// No. Use the minimum and maximum position as is.
// Get the minimum and maximum seek position.
min_position = registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
max_position = registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
// Make sure these values are sane 10-bit values.
if (min_position > MAX_POSITION) min_position = MAX_POSITION;
if (max_position > MAX_POSITION) max_position = MAX_POSITION;
}
// Disable clockwise movements when position is below the minimum position.
if ((position < min_position) && (pwm < 0)) pwm = 0;
// Disable counter-clockwise movements when position is above the maximum position.
if ((position > max_position) && (pwm > 0)) pwm = 0;
// Determine if PWM is disabled in the registers.
if (!(registers_read_byte(REG_FLAGS_LO) & (1<<FLAGS_LO_PWM_ENABLED))) pwm = 0;
// Determine direction of servo movement or stop.
if (pwm < 0)
{
// Less than zero. Turn clockwise.
// Get the PWM width from the PWM value.
pwm_width = (uint8_t) -pwm;
// Turn clockwise.
#if SWAP_PWM_DIRECTION_ENABLED
pwm_dir_b(pwm_width);
#else
pwm_dir_a(pwm_width);
#endif
}
else if (pwm > 0)
{
// More than zero. Turn counter-clockwise.
// Get the PWM width from the PWM value.
pwm_width = (uint8_t) pwm;
// Turn counter-clockwise.
#if SWAP_PWM_DIRECTION_ENABLED
pwm_dir_a(pwm_width);
#else
pwm_dir_b(pwm_width);
#endif
}
else
{
// Stop all PWM activity to the motor.
pwm_stop();
}
}
void pwm_update(uint16_t position, int16_t pwm)
// Update the PWM signal being sent to the motor. The PWM value should be
// a signed integer in the range of -255 to -1 for clockwise movement,
// 1 to 255 for counter-clockwise movement or zero to stop all movement.
// This function provides a sanity check against the servo position and
// will prevent the servo from being driven past a minimum and maximum
// position.
{
uint8_t pwm_width;
uint16_t min_position;
uint16_t max_position;
// Quick check to see if the frequency divider changed. If so we need to
// configure a new top value for timer/counter1. This value should only
// change infrequently so we aren't too elegant in how we handle updating
// the value. However, we need to be careful that we don't configure the
// top to a value lower than the counter and compare values.
if (registers_read_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO) != pwm_div)
{
// Disable OC1A and OC1B outputs.
TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0));
TCCR1A &= ~((1<<COM1B1) | (1<<COM1B0));
// Clear PB1 and PB2.
PORTB &= ~((1<<PB1) | (1<<PB2));
delay_loop(DELAYLOOP);
// Reset the A and B direction flags.
pwm_a = 0;
pwm_b = 0;
// Update the pwm frequency divider value.
pwm_div = registers_read_word(REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO);
// Update the timer top value.
ICR1 = PWM_TOP_VALUE(pwm_div);
// Reset the counter and compare values to prevent problems with the new top value.
TCNT1 = 0;
OCR1A = 0;
OCR1B = 0;
}
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Swap the minimum and maximum position.
// Get the minimum and maximum seek position.
min_position = registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
max_position = registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
// Make sure these values are sane 10-bit values.
if (min_position > 0x3ff) min_position = 0x3ff;
if (max_position > 0x3ff) max_position = 0x3ff;
// Adjust the values because of the reverse sense.
min_position = 0x3ff - min_position;
max_position = 0x3ff - max_position;
}
else
{
// No. Use the minimum and maximum position as is.
// Get the minimum and maximum seek position.
min_position = registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
max_position = registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
// Make sure these values are sane 10-bit values.
if (min_position > 0x3ff) min_position = 0x3ff;
if (max_position > 0x3ff) max_position = 0x3ff;
}
// Disable clockwise movements when position is below the minimum position.
if ((position < min_position) && (pwm < 0)) pwm = 0;
// Disable counter-clockwise movements when position is above the maximum position.
if ((position > max_position) && (pwm > 0)) pwm = 0;
// Determine if PWM is disabled in the registers.
if (!(registers_read_byte(REG_FLAGS_LO) & (1<<FLAGS_LO_PWM_ENABLED))) pwm = 0;
// Determine direction of servo movement or stop.
if (pwm < 0)
{
// Less than zero. Turn clockwise.
// Get the PWM width from the PWM value.
pwm_width = (uint8_t) -pwm;
// Turn clockwise.
#if SWAP_PWM_DIRECTION_ENABLED
pwm_dir_a(pwm_width);
#else
pwm_dir_b(pwm_width);
#endif
}
else if (pwm > 0)
{
// More than zero. Turn counter-clockwise.
// Get the PWM width from the PWM value.
pwm_width = (uint8_t) pwm;
// Turn counter-clockwise.
#if SWAP_PWM_DIRECTION_ENABLED
pwm_dir_b(pwm_width);
#else
pwm_dir_a(pwm_width);
#endif
}
else
{
// Stop all PWM activity to the motor.
pwm_stop();
}
}
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 step_update(uint16_t position, int16_t step_in)
// Update the timer delay that trigers a step. The delay time is determined by the step value, which represents a value of the maximum delay.
// The farther the step value is from zero, the longer the delay.
{
uint16_t min_position;
uint16_t max_position;
uint8_t step_mode;
static uint8_t prev_step_mode;
static uint16_t prev_pwm_div;
static int16_t prev_seek_position;
static uint8_t step_accel_idx = 0;
uint16_t duty_cycle;
uint16_t pwm_div_hi;
uint16_t pwm_div_lo;
// Read the high and low values for the PWM frequencies. pwm_div_hi is the top frequency and
// pwm_div_lo is the bottom frequency
pwm_div_hi = (uint16_t)banks_read_byte(CONFIG_BANK, REG_PWM_FREQ_DIVIDER_HI);
pwm_div_lo = (uint16_t)banks_read_byte(CONFIG_BANK, REG_PWM_FREQ_DIVIDER_LO);
// Store these in the correct 16 bit size
pwm_div_hi = (uint8_t)pwm_div_hi << 8;
pwm_div_lo = (uint8_t)pwm_div_lo << 8;
// Set up for the configured step mode
step_mode = banks_read_byte(CONFIG_BANK, REG_STEP_MODE);
// Check to see if the step mode has changed in the registers on the fly
if (prev_step_mode != step_mode)
step_init();
prev_step_mode = step_mode;
min_position = banks_read_word(CONFIG_BANK, REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
max_position = banks_read_word(CONFIG_BANK, REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
// Make sure these values are sane 10-bit values.
if (min_position > 0x3ff) min_position = 0x3ff;
if (max_position > 0x3ff) max_position = 0x3ff;
// Disable clockwise movements when position is below the minimum position.
if ((position < min_position) && (step_in < 0)) step_in = 0;
// Disable counter-clockwise movements when position is above the maximum position.
if ((position > max_position) && (step_in > 0)) step_in = 0;
// Determine if Stepping is disabled in the registers.
// TODO cheeck for braking and disable bridge
if (!(registers_read_byte(REG_FLAGS_LO) & (1<<FLAGS_LO_PWM_ENABLED))) step_in = 0;
// Determine and set the direction: Stop (0), Clockwise (1), Counter-Clockwise (2).
if (step_in < -5)
{
// Less than zero. Set the direction to clockwise.
direction |= R_COUNTER_CLOCKWISE;
direction &= ~R_CLOCKWISE;
// Calculate our duty cycle value
duty_cycle = PWM_OCRN_VALUE(pwm_div_hi, pwm_div_lo, -step_in);
}
else if (step_in > 5)
{
// More than zero. Set the direction to counter-clockwise.
direction |= R_CLOCKWISE; //DIRECTION = 2
direction &= ~R_COUNTER_CLOCKWISE;
// Calculate our duty cycle value
duty_cycle = PWM_OCRN_VALUE(pwm_div_hi, pwm_div_lo, step_in);
}
else
{
// Stop all stepping output to the motor by setting motor outputs to low.
step_stop();
return;
}
// If acceleration mode is set, the current speed is divided to all incremental
// speed. This function also waits for the R_ACCEL_CLEAR flag from the timer interrupt
// before starting the next increment. It should take 10 pulses to get up to speed.
if ((direction & R_ACCEL) && (direction & R_ACCEL_CLEAR))
{
// Scale the current duty cycle by 64 and increment over a loop
duty_cycle = (duty_cycle/64)*(step_accel_idx+1);
step_accel_idx++;
if (step_accel_idx >= 64)
{
step_accel_idx = 0;
direction &= ~R_ACCEL;
}
// clear the acceleration flag ready for next toggle
direction &= ~R_ACCEL_CLEAR;
}
// If the motor changes direction we need to wait a small amount of time to discharge,
// and then start accelerating in the new direction.
uint16_t seek_position = registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
if (seek_position != prev_seek_position)
{
direction |= R_ACCEL;
step_accel_idx = 0;
OCR1A = 6500; // Wait 1ms (at 20mhz)
}
else
{
// A slower speed is a larger delay, so calculate the top pwm frequency, and take away the step delay.
OCR1A = 65535 - pwm_div_lo - duty_cycle; //Update the CTC compare value with the modified value of step.
}
prev_seek_position = seek_position;
// Enable the timer mask
TIMSK1 |= (1 << OCIE1A);
}
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);
}
