void pwm_stop(void)
// Stop all PWM signals to the motor.
{
// Disable interrupts.
cli();
// Are we moving in the A or B direction?
if (pwm_a || pwm_b)
{
// 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;
}
// Set the PWM duty cycle to zero.
OCR1A = 0;
OCR1B = 0;
// Restore interrupts.
sei();
// Save the pwm A and B duty values.
registers_write_byte(REG_PWM_DIRA, pwm_a);
registers_write_byte(REG_PWM_DIRB, pwm_b);
}
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 pwm_stop(void)
// Stop all PWM signals to the motor.
{
// Disable interrupts.
cli();
// Are we moving in the A or B direction?
if (pwm_a || pwm_b)
{
// Make sure that SMPLn_B (PD4) and SMPLn_A (PD7) are held high.
PORTD |= ((1<<PD4) | (1<<PD7));
// Disable PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) output.
TCCR1A = 0;
// Make sure that PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) are held low.
PORTB &= ~((1<<PB1) | (1<<PB2));
// Do we want to enable braking?
if (1)
{
// Before enabling braking (which turns on the "two lower MOSFETS"), introduce
// sufficient delay to give the H-bridge time to respond to the change of state
// that has just been made.
delay_loop(DELAYLOOP);
// Hold EN_A (PD2) and EN_B (PD3) high.
PORTD |= ((1<<PD2) | (1<<PD3));
}
else
{
// Hold EN_A (PD2) and EN_B (PD3) low.
PORTD &= ~((1<<PD2) | (1<<PD3));
}
// Reset the A and B direction flags.
pwm_a = 0;
pwm_b = 0;
}
// Set the PWM duty cycle to zero.
OCR1A = 0;
OCR1B = 0;
// Restore interrupts.
sei();
// Save the pwm A and B duty values.
registers_write_byte(REG_PWM_DIRA, pwm_a);
registers_write_byte(REG_PWM_DIRB, pwm_b);
}
static void pwm_dir_a(uint8_t pwm_duty)
// Send PWM signal for rotation with the indicated pwm ratio (0 - 255).
// This function is meant to be called only by pwm_update.
{
// Determine the duty cycle value for the timer.
uint16_t duty_cycle = PWM_OCRN_VALUE(pwm_div, pwm_duty);
// Disable interrupts.
cli();
// Do we need to reconfigure PWM output?
if (!pwm_a || pwm_b)
{
// Disable PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) output.
// NOTE: Actually PWM_A should already be disabled...
TCCR1A &= ~((1<<COM1A1) | (1<<COM1B1));
OCR1A = duty_cycle;
// Yes. Make sure PB1 and PB2 are zero.
PORTB &= ~((1<<PB1) | (1<<PB2));
//
// Give the H-bridge time to respond to the above changes
//
delay_loop(DELAYLOOP);
// Enable PWM_A (PB1/OC1A) output.
TCCR1A |= (1<<COM1A1);
// Reset the B direction flag.
pwm_b = 0;
}
// Set the A direction flag.
pwm_a = pwm_duty;
// Update the PWM duty cycle.
OCR1A = duty_cycle;
OCR1B = 0;
// Restore interrupts.
sei();
// Save the pwm A and B duty values.
registers_write_byte(REG_PWM_DIRA, pwm_a);
registers_write_byte(REG_PWM_DIRB, pwm_b);
}
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 general_call_disable(void)
{
TWAR &= ~(1<<TWGCE); // disable general call
uint8_t flags_lo = registers_read_byte(REG_FLAGS_LO);
registers_write_byte(REG_FLAGS_LO, flags_lo & ~(1<<FLAGS_LO_GENERALCALL_ENABLED));
}
static void pwm_dir_a(uint16_t pwm_duty)
// Send PWM signal for rotation with the indicated pwm ratio (0 - 255).
// This function is meant to be called only by pwm_update.
{
// Determine the duty cycle value for the timer.
uint16_t duty_cycle = pwm_duty;
// Disable interrupts.
//nvic_globalirq_disable();
// Do we need to reconfigure PWM output?
if (!pwm_a || pwm_b)
{
// Disable PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) output.
// NOTE: Actually PWM_A should already be disabled...
pinMode(6, OUTPUT); //PA8
pinMode(7, OUTPUT); //PA9
// Yes. Make sure PB1 and PB2 are zero.
digitalWrite(6, LOW);
digitalWrite(7, LOW);
//
// Give the H-bridge time to respond to the above changes
//
delay_loop(DELAYLOOP);
// Reset the B direction flag.
pwm_b = 0;
}
// Set the A direction flag.
pwm_a = pwm_duty;
SerialUSB.println(pwm_a);
// Update the PWM duty cycle.
pinMode(6, PWM); //PA8
pwmWrite(6, pwm_a);
// Restore interrupts.
// nvic_globalirq_enable();
// Save the pwm A and B duty values.
registers_write_byte(REG_PWM_DIRA, pwm_a);
registers_write_byte(REG_PWM_DIRB, pwm_b);
}
// general call functions
void general_call_enable(void)
{
TWAR |= (1<<TWGCE); // Enable general call at address 0x00
uint8_t flags_lo = registers_read_byte(REG_FLAGS_LO);
// Enable PWM to the servo motor.
registers_write_byte(REG_FLAGS_LO, flags_lo | (1<<FLAGS_LO_GENERALCALL_ENABLED));
}
void pwm_stop(void)
// Stop all PWM signals to the motor.
{
// Disable interrupts.
//nvic_globalirq_disable();
pinMode(6, OUTPUT); //PA8
pinMode(7, OUTPUT); //PA9
pwmWrite(6, 0); // set low
pwmWrite(7, 0); // set low
// Restore interrupts.
//nvic_globalirq_enable();
// Save the pwm A and B duty values.
registers_write_byte(REG_PWM_DIRA, pwm_a);
registers_write_byte(REG_PWM_DIRB, pwm_b);
}
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);
}
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 pwm_init(void)
// Initialize the PWM module for controlling a DC motor.
{
// Initialize the pwm frequency divider value.
// Initialize the pwm frequency divider value.
pwm_div = banks_read_word(POS_PID_BANK, REG_PWM_FREQ_DIVIDER_HI, REG_PWM_FREQ_DIVIDER_LO);
pwm_max = banks_read_byte(CONFIG_BANK, REG_PWM_MAX);
asm("nop");
asm("nop");
asm("nop");
pinMode(6, PWM); //PA8
pinMode(7, PWM); //PA9
pwmWrite(6, 0);
pwmWrite(7, 0);
// Update the pwm values.
registers_write_byte(REG_PWM_DIRA, 0);
registers_write_byte(REG_PWM_DIRB, 0);
}
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 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));
}
static void twi_registers_write(uint8_t address, uint8_t data)
// Write non-write protected registers. This function handles the
// writing of special registers such as unused registers, redirect and
// redirected registers.
{
// Mask the most significant bit of the address.
address &= 0x7F;
// Are we writing a read only register?
if (address <= MAX_READ_ONLY_REGISTER)
{
// Yes. Block the write.
return;
}
// Are we writing a read/write register?
if (address <= MAX_READ_WRITE_REGISTER)
{
// Yes. Complete the write.
registers_write_byte(address, data);
return;
}
// Is writing to the upper registers disabled?
if (registers_is_write_disabled())
{
// Yes. Block the write.
return;
}
// Are we writing a write protected register?
if (address <= MAX_WRITE_PROTECT_REGISTER)
{
// Yes. Complete the write if writes are enabled.
registers_write_byte(address, data);
return;
}
// Are we writing an unused register.
if (address <= MAX_UNUSED_REGISTER)
{
// Yes. Block the write.
return;
}
// Are we writing a redirect register.
if (address <= MAX_REDIRECT_REGISTER)
{
// Yes. Complete the write.
registers_write_byte(address - (MIN_REDIRECT_REGISTER - MIN_UNUSED_REGISTER), data);
return;
}
// Are we writing a redirected register?
if (address <= MAX_REDIRECTED_REGISTER)
{
// Adjust address to reference appropriate redirect register.
address = MIN_REDIRECT_REGISTER + (address - MIN_REDIRECTED_REGISTER);
// Get the address from the redirect register.
address = registers_read_byte(address - (MIN_REDIRECTED_REGISTER - MIN_UNUSED_REGISTER));
// Prevent infinite recursion.
if (address <= MAX_REDIRECT_REGISTER)
{
// Recursively write redirected address.
twi_registers_write(address, data);
return;
}
}
// All other writes are blocked.
return;
}
void general_call_start_wait_reset(void)
{
uint8_t flags_lo = registers_read_byte(REG_FLAGS_LO);
registers_write_byte(REG_FLAGS_LO, flags_lo & ~(1<<FLAGS_LO_GENERALCALL_WAIT));
}
void general_call_start_move(void)
{
uint8_t flags_lo = registers_read_byte(REG_FLAGS_LO);
registers_write_byte(REG_FLAGS_LO, flags_lo | (1<<FLAGS_LO_GENERALCALL_START));
}
static void pwm_dir_a(uint8_t pwm_duty)
// Send PWM signal for rotation with the indicated pwm ratio (0 - 255).
// This function is meant to be called only by pwm_update.
{
// Determine the duty cycle value for the timer.
uint16_t duty_cycle = PWM_OCRN_VALUE(pwm_div, pwm_duty);
// Disable interrupts.
cli();
// Do we need to reconfigure PWM output for direction A?
if (!pwm_a)
{ // Yes...
// Set SMPLn_B (PD4) and SMPLn_A (PD7) to high.
PORTD |= ((1<<PD4) | (1<<PD7));
// Set EN_B (PD3) to low.
PORTD &= ~(1<<PD3);
// Disable PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) output.
// NOTE: Actually PWM_A should already be disabled...
TCCR1A = 0;
// Make sure PWM_A (PB1/OC1A) and PWM_B (PB2/OC1B) are low.
PORTB &= ~((1<<PB1) | (1<<PB2));
// 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);
// Enable PWM_A (PB1/OC1A) output.
TCCR1A |= (1<<COM1A1);
// Set EN_A (PD2) to high.
PORTD |= (1<<PD2);
// NOTE: The PWM driven state of the H-bridge should not be switched to b-mode or braking
// without a suffient delay.
// Reset the B direction flag.
pwm_b = 0;
}
// Update the A direction flag. A non-zero value keeps us from
// recofiguring the PWM output A when it is already configured.
pwm_a = pwm_duty;
// Update the PWM duty cycle.
OCR1A = duty_cycle;
OCR1B = 0;
// Restore interrupts.
sei();
// Save the pwm A and B duty values.
registers_write_byte(REG_PWM_DIRA, pwm_a);
registers_write_byte(REG_PWM_DIRB, pwm_b);
}
static void twi_registers_write(uint8_t address, uint8_t data)
// Write non-write protected registers. This function handles the
// writing of special registers such as unused registers, redirect and
// redirected registers.
{
// Mask the most significant bit of the address.
address &= 0x7F;
// Are we writing a read only register?
if (address <= MAX_READ_ONLY_REGISTER)
{
// Yes. Block the write.
return;
}
// Are we writing a read/write register?
if (address <= MAX_READ_WRITE_REGISTER)
{
// Yes. Complete the write.
registers_write_byte(address, data);
return;
}
// Is writing to the upper registers disabled?
if (registers_is_write_disabled())
{
// Yes. Block the write.
return;
}
// Are we writing a write protected register?
if (address <= MAX_WRITE_PROTECT_REGISTER)
{
// Yes. Complete the write if writes are enabled.
registers_write_byte(address, data);
return;
}
// Are we writing an unused register.
if (address <= MAX_UNUSED_REGISTER)
{
// Yes. Block the write.
return;
}
// Check that the bank selection is in range
if (address == REG_BANK_SELECT)
{
if (data >= MAX_BANKS)
{
// Set the bank number to the maximum
data = MAX_BANKS-1;
}
registers_write_byte(address, data);
return;
}
// Are we writing a bank register?
if (address <= MAX_BANK_REGISTER)
{
switch(banks_selected_bank())
{
case BANK_0:
break;
case BANK_1:
break;
case BANK_2:
// Are we writing a redirect register.
if (address <= MIN_BANK_REGISTER + MAX_REDIRECT_REGISTER)
{
// Yes. Complete the write.
banks_write_byte(REDIRECTED_BANK, address - MIN_BANK_REGISTER, data);
return;
}
// Are we writing a redirected register?
if (address <= MIN_BANK_REGISTER + MAX_REDIRECTED_REGISTER)
{
// Adjust address to reference appropriate redirect register.
address = address - (MIN_BANK_REGISTER + MIN_REDIRECTED_REGISTER);
// Get the address from the redirect register.
address = banks_read_byte(REDIRECTED_BANK, address);
// Prevent infinite recursion.
if (address <= MIN_BANK_REGISTER + MAX_REDIRECT_REGISTER)
{
// Recursively write redirected address.
twi_registers_write(address, data);
return;
}
}
break;
default:
return;
}
return banks_write_byte(banks_selected_bank(), address-MIN_BANK_REGISTER, data);
}
// All other writes are blocked.
return;
}
