DCMotor

StaticVars:
static uint32_t PWMPeriodInMicroSec
static uint8_t RobotDirection
static uint16_t LastLeftDutyCycle
static uint16_t LastRightDutyCycle
static uint32_t LeftEncoderPeriod
static uint32_t LastLeftCapture
static uint32_t RightEncoderPeriod
static uint32_t LasRightCapture
static uint8_t LeftRPMError
static uint8_t LeftTargetRPM
static uint8_t RightRPMError
static uint8_t RightTargetRPM
static uint32_t TargetX
static uint32_t TargetY
static uint32_t TargetHeading

Public Functions:
void InitDCMotor (void)
void SetDutyCycle(uint16_t DutyCycle)
void SetRobotDirection(uint8_t Direction)
void SetTargetPosition(GoalX, GoalY)
void SetTargetHeading(GoalTheta)
void InterruptLeftEncoder(void)
void InterruptRightEncoder(void)
void InterruptSpeedUpdate(void)

Private Functions
static void InitPWM (void)
static void InitEncoderCapture(void)
static void InitSpeedUpdater(void)

void InitDCMotor(void)
Initialize PWM hardware
Initialize Encoder Capture
Initialize Closed Loop Speed Control Timer
Initialize H-Bridge Control

void SetDutyCycle (WhichMotor, DutyCycle)

If WhichMotor is Left, Adjust Left PWM. If WhichMotor is Right, Adjust Right PWM
If DutyCycle is not 100 or Zero
Return GenB to Normal Mode
Set Compare Register of Appropriate PWM Cycle to Scaled Duty Cycle Number
If DutyCycle is 100
Set GenB to output 1 on first edge
If DutyCycle is 0
Set GenB to output 0 on first edge

void SetRobotDirection (Direction)

 If Direction is Forward
//Set Left Motor to CounterClockwise
Set Left Motor Non-PWM H-Bridge pin to 0
Set Left Motor PWM to NonInverted
//Set Right Motor to Clockwise
Set Right Motor Non-PWM H-Bridge pin to 1
Set Right Motor PWM to Inverted
If Direction is Backwards
//Set Left Motor to Clockwise
Set Left Motor Non-PWM H-Bridge pin to 1
Set Left Motor PWM to Inverted
//Set Right Motor to CounterClockwise
Set Right Motor Non-PWM H-Bridge pin to 0
Set Right Motor PWM to NonInverted
If Direction is CounterClockwise Turn
//Set Left Motor to CounterClockwise
Set Left Motor Non-PWM H-Bridge pin to 0
Set Left Motor PWM to NonInverted
//Set Right Motor to CounterClockwise
Set Right Motor Non-PWM H-Bridge pin to 0
Set Right Motor PWM to NonInverted
If Direction is Clockwise Turn
//Set Left Motor to Clockwise
Set Left Motor Non-PWM H-Bridge pin to 1
Set Left Motor PWM to Inverted
//Set Right Motor to Clockwise
Set Right Motor Non-PWM H-Bridge pin to 1
Set Right Motor PWM to Inverted

void SetTargetPosition(GoalX, GoalY)

TargetX = GoalX
TargetY = GoalY

Void SetTargetHeading(GoalTheta)
TargetHeading = GoalTheta

void StopMotors(void)
Set both motors duty cycle to zero
Stop Closed loop timers
Clear RPM error terms for both motors

void InterruptLeftEncoder(void)
Clear source of interrupt
Set CurrentEncoderCapture to current value of Timer A Register
Set EncoderPeriod between events using abs(CurrentEncoderCapture-
LastEncoderCapture)
Update LastEncoderCapture with value of CurrentEncoderCapture
// Encoder Dead Reckoning Position Tracking
If Direction is Foward
Increase X position by DistancePerPulse * cos(Current Heading)
 Increase Y position by DistancePerPulse * sin(Current Heading)
If Direction is Backwards
Decrease X position by DistancePerPulse * cos(Current Heading)
Decrease Y position by DistancePerPulse * sin(Current Heading)
If Direction is Counter-Clockwise Turn
Increase heading angle by DistancePerPulse / TurnRadius
If Direction is Clockwise Turn
Decrease heading angle by DistancePerPulse / TurnRadius

void InterruptRightEncoder(void)
Clear source of interrupt
Set CurrentEncoderCapture to current value of Timer A Register
Set EncoderPeriod between events using abs(CurrentEncoderCapture-
LastEncoderCapture)
Update LastEncoderCapture with value of CurrentEncoderCapture

void InterruptSpeedUpdate(void)
For Each Motor (Left, Right):
Clear Interrupt Source
Use Period to find Current RPM
Calculate RPMError as Target RPM – Current RPM
Add to total Integral Term
Clamp integral term to prevent windup
Set Duty cycle based on PID constants and control law
Update Last Error

static void InitPWM (void)

// Enable Clock to PWM (Module 0)
// Set PWM Clock as System Clock / 32
// Wait for PWM module clock to initialize
// Disable PWM to change settings
// Enable generator to set line to 1 on rising comp A event, 0 on falling comp A event
// Set PWM Period. For Up/Down count, load register should contain half the desired total
period
// Set initial default Duty Cycle on comp A just in case
// Enable Alternate Functions
// Configure Appropriate Ports to be PWM Output
// Enable Appropriate Ports as outputs
//Set PWM count mode and local sync for generator

static void InitEncoderCapture(void)

Enable clock to timer
Enable clock to Port D
Disable Widetimers 0
Set up timers 0 in 32 bit individual mode
Initialize interval load register to 0xffffffff
Set up Timer A and Timer B for capture mode, counting up, recording edge time
//One timer per wheel
 Set event type on Timer A and Timer B for Rising Edge
Complete hardware port initializations
Map Alternate functions for Input Capture on the correct pins
Enable local interrupts in register and in NVIC
Enable Global Interrupts
Enable wide timers with stall while debugging option on

static void InitSpeedUpdater(void)

Enable clock to timer
Disable Widetimer 3
Set up timers 3 in 32 bit individual mode
Initialize interval load register to 2 ms
Set up Timer A for periodic timer mode
Enable local interrupts in register and in NVIC
Enable Global Interrupts
Enable wide timers with stall while debugging option on