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