David's line following code from the LVBots competition, 2015.

Dependencies:   GeneralDebouncer Pacer PololuEncoder mbed

Fork of DeadReckoning by David Grayson

motors.cpp

Committer:
DavidEGrayson
Date:
2015-04-16
Revision:
57:99bec7fab454
Parent:
40:e79cefc241f8

File content as of revision 57:99bec7fab454:

#include <mbed.h>
#include "motors.h"

// Application     mbed pin    LPC1768
// Motor L PWM     p26         P2[0]/PWM1[1]
// Motor L dir     p25
// Motor R PWM     p24         P2[2]/PWM1[3]
// Motor R dir     p23

// Clock structure:
// System clock: 96 MHz
// In LPC_SC->PCLKSEL0, PWM is assigned to system clock / 4 by default, so it ticks at 24 MHz.
// This allows us to have 1200 possible speeds at 20 kHz.

DigitalOut motorLeftDir(p25);
DigitalOut motorRightDir(p23);

int16_t motorLeftSpeed = 0;;
int16_t motorRightSpeed = 0;

void motorsInit()
{
    //PwmOut(p26).period_us(100);
    
    // Power the PWM module by setting PCPWM1 bit in PCONP register.  (Table 46).
    LPC_SC->PCONP |= (1 << 6);
    
    // In PCLKSEL0 register, set the clock for PWM1 to be equal to CCLK/4 (96/4 = 24 MHz).
    LPC_SC->PCLKSEL0 &= ~(3 << 12);

    // Select the functions of P2.0 and P2.2 as PWM.  (Table 83).
    LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & ~((3 << 0) | (3 << 4))) | ((1 << 0) | (1 << 4));
    
    // Set most parts of the PWM module to their defaults.
    LPC_PWM1->TCR = 0;
    LPC_PWM1->CTCR = 0;
    LPC_PWM1->CCR = 0;
    
    // Enable PWM output 1 and PWM output 3.
    LPC_PWM1->PCR = (1 << 9) | (1 << 11);

    LPC_PWM1->MCR = (1 << 1);   // Reset PWMTC when it is equal to MR0.
    
    LPC_PWM1->MR0 = 1200;       // Set the period.  This must be done before enabling PWM.
    LPC_PWM1->LER = (1 << 0);
    motorsSpeedSet(0, 0);
    
    LPC_PWM1->TCR = (1 << 0) | (1 << 3);   // Enable the PWM counter and enable PWM.
}

void motorsSpeedSet(int16_t newMotorLeftSpeed, int16_t newMotorRightSpeed)
{
    motorLeftSpeed = newMotorLeftSpeed;
    motorRightSpeed = newMotorRightSpeed;
    
    if (motorLeftSpeed < 0)
    {
        motorLeftSpeed = -motorLeftSpeed;
        motorLeftDir = 0;   
    }
    else
    {
        motorLeftDir = 1;
    }
    
    if (motorRightSpeed < 0)
    {
        motorRightSpeed = -motorRightSpeed;   
        motorRightDir = 0;
    }
    else
    {
        motorRightDir = 1;
    }
    LPC_PWM1->MR1 = motorLeftSpeed;
    LPC_PWM1->MR3 = motorRightSpeed;
    
    LPC_PWM1->LER |= (1<<1) | (1<<3);
}