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); }