Angels_MotorsModule.c

The .h file includes:

Module Definitions: 
pGain
iGain

Module Functions:
InitInterrupts()
TurnOnLEDs

Module Variables:
PeriodLA
PeriodLB
PeriodRA
PeriodRB
TargetR_RPM
TargetL_RPM
R_RequestedDuty
L_RequestedDuty
L_RPM
R_RPM
R_SumError
R_RPMError
R_ThisPeriod
L_SumError
L_RPMError
L_ThisPeriod

________________________________________________________________ 

InitMotorPorts() Function
/**********************************************************************
Function: InitMotorPorts
Parameters: None
Returns: None
Description: Sets ports used for motors.
**********************************************************************/

Set 4 output ports to drive motor.  
Set 2 inputs to read the encoders on each motor.
________________________________________________________________ 

InitPWM() Function
/*********************************************************************
Function: InitPWM
Parameters: None
Returns: NoneDescription: Initializes PWM

**********************************************************************/

Map two PWM channels
Scale the 24MHz clock by 2
Select clocks SA and SB
Scale clock to 20kHz (clock speed)/(x * prescaler * 2 * period)
Center align one of the channels
Set both periods to 100
________________________________________________________________ 

InitPIDInterrupts() Function
/**********************************************************************
Function: InitPIDInterrupts
Parameters: None
Returns: None
Description: Sets ports used for motors.
**********************************************************************/Enable timers 1 and 0
Prescale the timers by 16 => 1.5MHz clock

Initialize T4 as an input capture
Set IC4 to trigger an interrupt on the leading edge only
Reset the IC4 flag
Enable IC4

Initialize T5 as an input capture
Set IC5 to trigger an interrupt on the leading edge only
Reset the IC5 flag
Enable IC5

Initialize T6 as an input capture
Set IC6 to trigger an interrupt on the leading edge only
Reset the IC6 flag
Enable IC6

Initialize T7 as an input capture
Set IC7 to trigger an interrupt on the leading edge only
Reset the IC7 flag
Enable IC7

Initialize T3 to output compare
Schedule first event
Clear the OC3 flag
Enable OC3

Globally enable interrupts

________________________________________________________________ 

interrupt _Vec_tim1ch6 EncoderPeriodRA() Function
/**********************************************************************
Function: interrupt _Vec_tim1ch6 EncoderPeriodRA
Parameters: none
Returns: None
Description: Responds to Period Interrupt to calculate encoder period Left A
**********************************************************************

Initialize static variable LastEdgeRA
PeriodLA is equal to the current time – LastEdgeRA
Set LastEdgeRA equal to the current time
Reset the IC6 flag

________________________________________________________________ 

interrupt _Vec_tim1ch4 EncoderPeriodLA() Function
/**********************************************************************
Function: interrupt _Vec_tim1ch4 EncoderPeriodLA
Parameters: none
Returns: None
Description: Responds to Period Interrupt to calculate encoder period Left A
**********************************************************************

Initialize static variable LastEdgeLA
PeriodLA is equal to the current time – LastEdgeLA
Set LastEdgeLA equal to the current time
Reset the IC4 flag

________________________________________________________________ 

interrupt _Vec_tim0ch7 ControlLaw() Function
/**********************************************************************
Function: ControlLaw
Parameters: None
Returns: None
Description: Motor PID Control Law
**********************************************************************/

Reset timer
Reset OC3 flag
Store current period variables R_ThisPeriod equals PeriodRA
					 L_ThisPeriod equals PeriodLA
Globally enable interrupts
Calculate current RPMs based on periods
Calculate error between target RPMs and actual RPMs
Sum error for each right and left
Calculate a requested duty cycle for right and left using PI
Antiwindup clause for both right and left sides
Set PWM to requested duty cycle

________________________________________________________________ 

SetLeftMotor() Function
/**********************************************************************
Function: SetLeftMotor
Parameters: char to specify direction (0=FORWARD, 1 = BACKWARD)
char to specify duty cycle 0 to 100 (start is between 15 and 20) 
Returns: None
Description: Sets the direction and duty cycle of the LeftMotor
**********************************************************************/
Reset error terms for PI control
If direction is BACKWARDS
	Enable PWM on pin 0
	Set PWM polarity hi
	Set target RPM  for PI based on parameter
	Set direction pin 1 lo
else if direction is FORWARD
	Enable PWM on pin 0
	Set polarity HI
	Set target RPM for PI based on parameter
	Set direction pin 1 hi

________________________________________________________________ 

SetLRightMotor() Function
/**********************************************************************
Function: SetRighttMotor
Parameters: char to specify direction (0=FORWARD, 1 = BACKWARD)
char to specify duty cycle 0 to 100 (start is between 15 and 20) 
Returns: None
Description: Sets the direction and duty cycle of the RightMotor
**********************************************************************/
Reset error terms for PI control
If direction is BACKWARDS
	Enable PWM on pin 2
	Set PWM polarity hi
	Set target RPM  for PI based on parameter
	Set direction pin 3 lo
else if direction is FORWARD
	Enable PWM on pin 2
	Set polarity HI
	Set target RPM for PI based on parameter
	Set direction pin 3 hi
________________________________________________________________ 

MotorsOff() Function
/**********************************************************************
Function: SetRighttMotor
Parameters: None
Returns: None
Description: Turns off both motors
**********************************************************************/
Disable PWM for both motors.