Module
   ControlLibrary.c

 Description
   This is a library for all of the ISRs and functions relating to motor control

 Notes

 Private definitions

MOTOR_A_FORWARD 
MOTOR_A_BACKWARD 
MOTOR_B_FORWARD 
MOTOR_B_BACKWARD 
FORWARD_RPM 
BACKWARD_RPM 
ROTATE_RPM 
STOP_RPM 
LOWER_OFFSET 
UPPER_OFFSET 
RESET_RPM_TIME 
RESET_PERIOD 
MOTOR_STALL_THRESHOLD_RPM 
PeriodA
LastCaptureA
MotorSpeedA
SumErrorA
ProportionalGainA
IntegralGainA
RequestedDutyA 
RPMErrorA 
DirectionA 
PProportionalGainA 
PDerivativeGainA 
EncoderCountA 
LastEncoderErrorA 
EncoderTargetA
PeriodB
LastCaptureB
MotorSpeedB
SumErrorB
ProportionalGainB 
IntegralGainB 
RequestedDutyB 
RPMErrorB 
DirectionB 
PProportionalGainB 
PDerivativeGainB 
EncoderCountB 
LastEncoderErrorB 
EncoderTargetB 
MotorAStallCount 
MotorBStallCount

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/

void InitInputCapturePeriod(void)

	//Enable clock to WT0
	//Kill a few cycles
	//Enable clock to Port C 	
	//Disable timer A, B
	//Set up 32 bit mode
	//Use full 32 bit count
	//Set up timer A, B in capture mode
	//Set up to capture rising edges
	//Set up port to do capture
	//Map alternate functions
	//Enable pins on port c for digital I/O
	//Make pins into input
	//Enable local capture interrupt
	//Enable timer A in wide timer interupt in NVIC
	//Enable timer B
	//Enable interrupts globally
	//Kick timer off
	
void InitPeriodicTimers(void)
	//Enable clock to WT1
	//Kill a few cycles
	//Disable timer A, B
	//Set up 32 bit mode 
	//Use full 32 bit count 
	//Set up timer A,B in periodic mode 
	//Set timeout to 2ms 
	//Enable a local timeout interrupt 
	//Enable timer A in wide timer interrupt in NVIC 
	//Enable interrupts globally 
	//Kick timer off 
	
void InputCaptureResponse(void)

	//Clear source of interrupt
	//Reset timer 
	//Grab captured value
	//Increment EncoderCount 
	//Update lastCapture to prepare for next edge
	

void ControlPWMTimerResponse(void)	

	//Clear interrupt
	//If Velocity Control 
		//Query current RPM 
		//Calculate RPM error
		//If RPMError > MotorStallThreshold
			//Increment motor stall count
		//else
			//Reset motor stall count
		//If MotorStallCount reaches stall threshold
			//Reset motor stall count
			//Post collision detected event
		//Add to error sum
		//Calculate requested duty cycle 
		//If requested duty > 100
			//Set requested duty to 100
			//Subtract sum error
		//Else if requested duty less than 0
			//Subtract sum error
			//Set requested duty to 0
	//Else 
		//if Current encoder error is less than 5
			//Reset position control 
			//Set target RPM to 0
			//If other motor is in velocity control
				//Post rotate done event
		//Calculate requested duty cycle
		//Add offset
		//Update encoder error 
		//If requested duty is above upper offset
			//Set requested duty to upper offset
		//Else if requested duty is less than 0
			//Set requested duty to 0
	//Set duty cycle
	
int16_t getDuty(void)
	//return RequestedDuty
	
void Drive(bool Direction)
	//Set to velocity control
	//If direction is forward
		//Set motor directions forward
		//Set target rpm to forward rpm
	//Else
		//Set motor directions backward
		//Set target rpm to backward rpm
		
		
void Rotate(void) 	
	//Set to velocity control
	//Set motor directions based on rotate direction
	//Set target rpm to rotate rpm 
 
void RotateControl(int16_t HeadingChange)
	//Set to position control 
	//If heading change is less than 0
		//Set to rotate right
	//Else
		//Set to rotate left
	
	//Scale headingchange and set to encodertarget
	//Reset encodercount
	
void Stop(void)
	//Set to velocity control
	//Set target RPM to zero
	
int32_t QuerySpeedA(void)
	//Return motorspeed