David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
motors.cpp@27:2456f68be679, 2014-03-01 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Sat Mar 01 01:46:35 2014 +0000
- Revision:
- 27:2456f68be679
- Parent:
- 9:9734347b5756
- Child:
- 32:83a13b06093c
Fixed a major bug in the line following (reduceSpeed return value was not used). Made finalSettleIn better by adding an integral term and increasing the settleSpeed from 200 to 300.
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 | 9:9734347b5756 | 18 | void motorsInit() |
DavidEGrayson | 4:1b20a11765c8 | 19 | { |
DavidEGrayson | 8:78b1ff957cba | 20 | //PwmOut(p26).period_us(100); |
DavidEGrayson | 8:78b1ff957cba | 21 | |
DavidEGrayson | 8:78b1ff957cba | 22 | // Power the PWM module by setting PCPWM1 bit in PCONP register. (Table 46). |
DavidEGrayson | 8:78b1ff957cba | 23 | LPC_SC->PCONP |= (1 << 6); |
DavidEGrayson | 8:78b1ff957cba | 24 | |
DavidEGrayson | 8:78b1ff957cba | 25 | // In PCLKSEL0 register, set the clock for PWM1 to be equal to CCLK/4 (96/4 = 24 MHz). |
DavidEGrayson | 8:78b1ff957cba | 26 | LPC_SC->PCLKSEL0 &= ~(3 << 12); |
DavidEGrayson | 8:78b1ff957cba | 27 | |
DavidEGrayson | 8:78b1ff957cba | 28 | // Select the functions of P2.0 and P2.2 as PWM. (Table 83). |
DavidEGrayson | 8:78b1ff957cba | 29 | LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & ~((3 << 0) | (3 << 4))) | ((1 << 0) | (1 << 4)); |
DavidEGrayson | 7:85b8b5acfb22 | 30 | |
DavidEGrayson | 7:85b8b5acfb22 | 31 | // Set most parts of the PWM module to their defaults. |
DavidEGrayson | 7:85b8b5acfb22 | 32 | LPC_PWM1->TCR = 0; |
DavidEGrayson | 7:85b8b5acfb22 | 33 | LPC_PWM1->CTCR = 0; |
DavidEGrayson | 7:85b8b5acfb22 | 34 | LPC_PWM1->CCR = 0; |
DavidEGrayson | 5:01ad080dc4fa | 35 | |
DavidEGrayson | 7:85b8b5acfb22 | 36 | // Enable PWM output 1 and PWM output 3. |
DavidEGrayson | 7:85b8b5acfb22 | 37 | LPC_PWM1->PCR = (1 << 9) | (1 << 11); |
DavidEGrayson | 7:85b8b5acfb22 | 38 | |
DavidEGrayson | 5:01ad080dc4fa | 39 | LPC_PWM1->MCR = (1 << 1); // Reset PWMTC when it is equal to MR0. |
DavidEGrayson | 7:85b8b5acfb22 | 40 | |
DavidEGrayson | 7:85b8b5acfb22 | 41 | LPC_PWM1->MR0 = 1200; // Set the period. This must be done before enabling PWM. |
DavidEGrayson | 7:85b8b5acfb22 | 42 | LPC_PWM1->LER = (1 << 0); |
DavidEGrayson | 9:9734347b5756 | 43 | motorsSpeedSet(0, 0); |
DavidEGrayson | 7:85b8b5acfb22 | 44 | |
DavidEGrayson | 7:85b8b5acfb22 | 45 | LPC_PWM1->TCR = (1 << 0) | (1 << 3); // Enable the PWM counter and enable PWM. |
DavidEGrayson | 4:1b20a11765c8 | 46 | } |
DavidEGrayson | 4:1b20a11765c8 | 47 | |
DavidEGrayson | 9:9734347b5756 | 48 | void motorsSpeedSet(int16_t motorLeftSpeed, int16_t motorRightSpeed) |
DavidEGrayson | 4:1b20a11765c8 | 49 | { |
DavidEGrayson | 9:9734347b5756 | 50 | if (motorLeftSpeed < 0) |
DavidEGrayson | 8:78b1ff957cba | 51 | { |
DavidEGrayson | 9:9734347b5756 | 52 | motorLeftSpeed = -motorLeftSpeed; |
DavidEGrayson | 9:9734347b5756 | 53 | motorLeftDir = 0; |
DavidEGrayson | 8:78b1ff957cba | 54 | } |
DavidEGrayson | 8:78b1ff957cba | 55 | else |
DavidEGrayson | 8:78b1ff957cba | 56 | { |
DavidEGrayson | 9:9734347b5756 | 57 | motorLeftDir = 1; |
DavidEGrayson | 8:78b1ff957cba | 58 | } |
DavidEGrayson | 9:9734347b5756 | 59 | LPC_PWM1->MR1 = motorLeftSpeed; |
DavidEGrayson | 8:78b1ff957cba | 60 | |
DavidEGrayson | 9:9734347b5756 | 61 | if (motorRightSpeed < 0) |
DavidEGrayson | 8:78b1ff957cba | 62 | { |
DavidEGrayson | 9:9734347b5756 | 63 | motorRightSpeed = -motorRightSpeed; |
DavidEGrayson | 9:9734347b5756 | 64 | motorRightDir = 0; |
DavidEGrayson | 8:78b1ff957cba | 65 | } |
DavidEGrayson | 8:78b1ff957cba | 66 | else |
DavidEGrayson | 8:78b1ff957cba | 67 | { |
DavidEGrayson | 9:9734347b5756 | 68 | motorRightDir = 1; |
DavidEGrayson | 8:78b1ff957cba | 69 | } |
DavidEGrayson | 9:9734347b5756 | 70 | LPC_PWM1->MR3 = motorRightSpeed; |
DavidEGrayson | 8:78b1ff957cba | 71 | |
DavidEGrayson | 7:85b8b5acfb22 | 72 | LPC_PWM1->LER |= (1<<1) | (1<<3); |
DavidEGrayson | 4:1b20a11765c8 | 73 | } |