David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: motors.cpp
- Revision:
- 8:78b1ff957cba
- Parent:
- 7:85b8b5acfb22
- Child:
- 9:9734347b5756
--- a/motors.cpp Fri Feb 21 00:00:43 2014 +0000 +++ b/motors.cpp Sat Feb 22 02:23:21 2014 +0000 @@ -4,18 +4,29 @@ // Application mbed pin LPC1768 // Motor 1 PWM p26 P2[0]/PWM1[1] // Motor 1 dir p25 -// Motor 2 PWM p27 P2[2]/PWM1[3] -// Motor 2 dir p28 +// Motor 2 PWM p24 P2[2]/PWM1[3] +// Motor 2 dir p23 -static PwmOut motor1Pwm(p26); +// Clock structure: +// System clock: 96 MHz +// In LPC_SC->PCLKSEL0, PWM is assigned to system clock / 4 by default, so it ticks at 24 MHz. +// This allows us to have 1200 possible speeds at 20 kHz. + DigitalOut motor1Dir(p25); - -static PwmOut motor2Pwm(p24); DigitalOut motor2Dir(p23); void motors_init() { - motor1Pwm.period_us(100); + //PwmOut(p26).period_us(100); + + // Power the PWM module by setting PCPWM1 bit in PCONP register. (Table 46). + LPC_SC->PCONP |= (1 << 6); + + // In PCLKSEL0 register, set the clock for PWM1 to be equal to CCLK/4 (96/4 = 24 MHz). + LPC_SC->PCLKSEL0 &= ~(3 << 12); + + // Select the functions of P2.0 and P2.2 as PWM. (Table 83). + LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & ~((3 << 0) | (3 << 4))) | ((1 << 0) | (1 << 4)); // Set most parts of the PWM module to their defaults. LPC_PWM1->TCR = 0; @@ -32,12 +43,31 @@ motors_speed_set(0, 0); LPC_PWM1->TCR = (1 << 0) | (1 << 3); // Enable the PWM counter and enable PWM. - } void motors_speed_set(int16_t motor1_speed, int16_t motor2_speed) { + if (motor1_speed < 0) + { + motor1_speed = -motor1_speed; + motor1Dir = 0; + } + else + { + motor1Dir = 1; + } LPC_PWM1->MR1 = motor1_speed; + + if (motor2_speed < 0) + { + motor2_speed = -motor2_speed; + motor2Dir = 0; + } + else + { + motor2Dir = 1; + } LPC_PWM1->MR3 = motor2_speed; + LPC_PWM1->LER |= (1<<1) | (1<<3); }