motors.cpp

00001 #include <mbed.h>
00002 #include "motors.h"
00003 
00004 // Application     mbed pin    LPC1768
00005 // Motor L PWM     p26         P2[0]/PWM1[1]
00006 // Motor L dir     p25
00007 // Motor R PWM     p24         P2[2]/PWM1[3]
00008 // Motor R dir     p23
00009 
00010 // Clock structure:
00011 // System clock: 96 MHz
00012 // In LPC_SC->PCLKSEL0, PWM is assigned to system clock / 4 by default, so it ticks at 24 MHz.
00013 // This allows us to have 1200 possible speeds at 20 kHz.
00014 
00015 DigitalOut motorLeftDir(p25);
00016 DigitalOut motorRightDir(p23);
00017 
00018 int16_t motorLeftSpeed = 0;;
00019 int16_t motorRightSpeed = 0;
00020 
00021 void motorsInit()
00022 {
00023     //PwmOut(p26).period_us(100);
00024     
00025     // Power the PWM module by setting PCPWM1 bit in PCONP register.  (Table 46).
00026     LPC_SC->PCONP |= (1 << 6);
00027     
00028     // In PCLKSEL0 register, set the clock for PWM1 to be equal to CCLK/4 (96/4 = 24 MHz).
00029     LPC_SC->PCLKSEL0 &= ~(3 << 12);
00030 
00031     // Select the functions of P2.0 and P2.2 as PWM.  (Table 83).
00032     LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & ~((3 << 0) | (3 << 4))) | ((1 << 0) | (1 << 4));
00033     
00034     // Set most parts of the PWM module to their defaults.
00035     LPC_PWM1->TCR = 0;
00036     LPC_PWM1->CTCR = 0;
00037     LPC_PWM1->CCR = 0;
00038     
00039     // Enable PWM output 1 and PWM output 3.
00040     LPC_PWM1->PCR = (1 << 9) | (1 << 11);
00041 
00042     LPC_PWM1->MCR = (1 << 1);   // Reset PWMTC when it is equal to MR0.
00043     
00044     LPC_PWM1->MR0 = 1200;       // Set the period.  This must be done before enabling PWM.
00045     LPC_PWM1->LER = (1 << 0);
00046     motorsSpeedSet(0, 0);
00047     
00048     LPC_PWM1->TCR = (1 << 0) | (1 << 3);   // Enable the PWM counter and enable PWM.
00049 }
00050 
00051 void motorsSpeedSet(int16_t newMotorLeftSpeed, int16_t newMotorRightSpeed)
00052 {
00053     motorLeftSpeed = newMotorLeftSpeed;
00054     motorRightSpeed = newMotorRightSpeed;
00055     
00056     if (motorLeftSpeed < 0)
00057     {
00058         motorLeftSpeed = -motorLeftSpeed;
00059         motorLeftDir = 0;   
00060     }
00061     else
00062     {
00063         motorLeftDir = 1;
00064     }
00065     
00066     if (motorRightSpeed < 0)
00067     {
00068         motorRightSpeed = -motorRightSpeed;   
00069         motorRightDir = 0;
00070     }
00071     else
00072     {
00073         motorRightDir = 1;
00074     }
00075     LPC_PWM1->MR1 = motorLeftSpeed;
00076     LPC_PWM1->MR3 = motorRightSpeed;
00077     
00078     LPC_PWM1->LER |= (1<<1) | (1<<3);
00079 }