David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
motors.cpp@32:83a13b06093c, 2014-03-04 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Tue Mar 04 04:32:51 2014 +0000
- Revision:
- 32:83a13b06093c
- Parent:
- 9:9734347b5756
- Child:
- 40:e79cefc241f8
getting line sensors to work again;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 4:1b20a11765c8 | 1 | #include <mbed.h> |
DavidEGrayson | 7:85b8b5acfb22 | 2 | #include "motors.h" |
DavidEGrayson | 7:85b8b5acfb22 | 3 | |
DavidEGrayson | 7:85b8b5acfb22 | 4 | // Application mbed pin LPC1768 |
DavidEGrayson | 9:9734347b5756 | 5 | // Motor L PWM p26 P2[0]/PWM1[1] |
DavidEGrayson | 9:9734347b5756 | 6 | // Motor L dir p25 |
DavidEGrayson | 9:9734347b5756 | 7 | // Motor R PWM p24 P2[2]/PWM1[3] |
DavidEGrayson | 9:9734347b5756 | 8 | // Motor R dir p23 |
DavidEGrayson | 4:1b20a11765c8 | 9 | |
DavidEGrayson | 8:78b1ff957cba | 10 | // Clock structure: |
DavidEGrayson | 8:78b1ff957cba | 11 | // System clock: 96 MHz |
DavidEGrayson | 8:78b1ff957cba | 12 | // In LPC_SC->PCLKSEL0, PWM is assigned to system clock / 4 by default, so it ticks at 24 MHz. |
DavidEGrayson | 8:78b1ff957cba | 13 | // This allows us to have 1200 possible speeds at 20 kHz. |
DavidEGrayson | 8:78b1ff957cba | 14 | |
DavidEGrayson | 9:9734347b5756 | 15 | DigitalOut motorLeftDir(p25); |
DavidEGrayson | 9:9734347b5756 | 16 | DigitalOut motorRightDir(p23); |
DavidEGrayson | 4:1b20a11765c8 | 17 | |
DavidEGrayson | 32:83a13b06093c | 18 | int16_t motorLeftSpeed = 0;; |
DavidEGrayson | 32:83a13b06093c | 19 | int16_t motorRightSpeed = 0; |
DavidEGrayson | 32:83a13b06093c | 20 | |
DavidEGrayson | 9:9734347b5756 | 21 | void motorsInit() |
DavidEGrayson | 4:1b20a11765c8 | 22 | { |
DavidEGrayson | 8:78b1ff957cba | 23 | //PwmOut(p26).period_us(100); |
DavidEGrayson | 8:78b1ff957cba | 24 | |
DavidEGrayson | 8:78b1ff957cba | 25 | // Power the PWM module by setting PCPWM1 bit in PCONP register. (Table 46). |
DavidEGrayson | 8:78b1ff957cba | 26 | LPC_SC->PCONP |= (1 << 6); |
DavidEGrayson | 8:78b1ff957cba | 27 | |
DavidEGrayson | 8:78b1ff957cba | 28 | // In PCLKSEL0 register, set the clock for PWM1 to be equal to CCLK/4 (96/4 = 24 MHz). |
DavidEGrayson | 8:78b1ff957cba | 29 | LPC_SC->PCLKSEL0 &= ~(3 << 12); |
DavidEGrayson | 8:78b1ff957cba | 30 | |
DavidEGrayson | 8:78b1ff957cba | 31 | // Select the functions of P2.0 and P2.2 as PWM. (Table 83). |
DavidEGrayson | 8:78b1ff957cba | 32 | LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & ~((3 << 0) | (3 << 4))) | ((1 << 0) | (1 << 4)); |
DavidEGrayson | 7:85b8b5acfb22 | 33 | |
DavidEGrayson | 7:85b8b5acfb22 | 34 | // Set most parts of the PWM module to their defaults. |
DavidEGrayson | 7:85b8b5acfb22 | 35 | LPC_PWM1->TCR = 0; |
DavidEGrayson | 7:85b8b5acfb22 | 36 | LPC_PWM1->CTCR = 0; |
DavidEGrayson | 7:85b8b5acfb22 | 37 | LPC_PWM1->CCR = 0; |
DavidEGrayson | 5:01ad080dc4fa | 38 | |
DavidEGrayson | 7:85b8b5acfb22 | 39 | // Enable PWM output 1 and PWM output 3. |
DavidEGrayson | 7:85b8b5acfb22 | 40 | LPC_PWM1->PCR = (1 << 9) | (1 << 11); |
DavidEGrayson | 7:85b8b5acfb22 | 41 | |
DavidEGrayson | 5:01ad080dc4fa | 42 | LPC_PWM1->MCR = (1 << 1); // Reset PWMTC when it is equal to MR0. |
DavidEGrayson | 7:85b8b5acfb22 | 43 | |
DavidEGrayson | 7:85b8b5acfb22 | 44 | LPC_PWM1->MR0 = 1200; // Set the period. This must be done before enabling PWM. |
DavidEGrayson | 7:85b8b5acfb22 | 45 | LPC_PWM1->LER = (1 << 0); |
DavidEGrayson | 9:9734347b5756 | 46 | motorsSpeedSet(0, 0); |
DavidEGrayson | 7:85b8b5acfb22 | 47 | |
DavidEGrayson | 7:85b8b5acfb22 | 48 | LPC_PWM1->TCR = (1 << 0) | (1 << 3); // Enable the PWM counter and enable PWM. |
DavidEGrayson | 4:1b20a11765c8 | 49 | } |
DavidEGrayson | 4:1b20a11765c8 | 50 | |
DavidEGrayson | 32:83a13b06093c | 51 | void motorsSpeedSet(int16_t newMotorLeftSpeed, int16_t newMotorRightSpeed) |
DavidEGrayson | 4:1b20a11765c8 | 52 | { |
DavidEGrayson | 32:83a13b06093c | 53 | motorLeftSpeed = newMotorLeftSpeed; |
DavidEGrayson | 32:83a13b06093c | 54 | motorRightSpeed = newMotorRightSpeed; |
DavidEGrayson | 32:83a13b06093c | 55 | |
DavidEGrayson | 9:9734347b5756 | 56 | if (motorLeftSpeed < 0) |
DavidEGrayson | 8:78b1ff957cba | 57 | { |
DavidEGrayson | 9:9734347b5756 | 58 | motorLeftSpeed = -motorLeftSpeed; |
DavidEGrayson | 9:9734347b5756 | 59 | motorLeftDir = 0; |
DavidEGrayson | 8:78b1ff957cba | 60 | } |
DavidEGrayson | 8:78b1ff957cba | 61 | else |
DavidEGrayson | 8:78b1ff957cba | 62 | { |
DavidEGrayson | 9:9734347b5756 | 63 | motorLeftDir = 1; |
DavidEGrayson | 8:78b1ff957cba | 64 | } |
DavidEGrayson | 9:9734347b5756 | 65 | LPC_PWM1->MR1 = motorLeftSpeed; |
DavidEGrayson | 8:78b1ff957cba | 66 | |
DavidEGrayson | 9:9734347b5756 | 67 | if (motorRightSpeed < 0) |
DavidEGrayson | 8:78b1ff957cba | 68 | { |
DavidEGrayson | 9:9734347b5756 | 69 | motorRightSpeed = -motorRightSpeed; |
DavidEGrayson | 9:9734347b5756 | 70 | motorRightDir = 0; |
DavidEGrayson | 8:78b1ff957cba | 71 | } |
DavidEGrayson | 8:78b1ff957cba | 72 | else |
DavidEGrayson | 8:78b1ff957cba | 73 | { |
DavidEGrayson | 9:9734347b5756 | 74 | motorRightDir = 1; |
DavidEGrayson | 8:78b1ff957cba | 75 | } |
DavidEGrayson | 9:9734347b5756 | 76 | LPC_PWM1->MR3 = motorRightSpeed; |
DavidEGrayson | 8:78b1ff957cba | 77 | |
DavidEGrayson | 7:85b8b5acfb22 | 78 | LPC_PWM1->LER |= (1<<1) | (1<<3); |
DavidEGrayson | 4:1b20a11765c8 | 79 | } |