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@21:c279c6a83671, 2014-02-27 (annotated)

Committer:
DavidEGrayson
Date:
Thu Feb 27 23:20:34 2014 +0000
Revision:
21:c279c6a83671
Parent:
9:9734347b5756
Child:
32:83a13b06093c
Wrote a whole bunch of code that could theoretically allow the robot to compete, but it has not been tested at all yet.

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