David Grayson / Mbed 2 deprecated DeadReckoning

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@38:5e93a479c244, 2014-03-13 (annotated)

Committer:
DavidEGrayson
Date:
Thu Mar 13 17:49:43 2014 +0000
Revision:
38:5e93a479c244
Parent:
32:83a13b06093c 
The final version of the code that was used for the competition.;

Who changed what in which revision?

UserRevisionLine numberNew 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 }