Module
   MotorService.c

 

 Description
   Implements motor control for the BARGE

 Notes

Private definitions

PROP_GAIN 
INT_GAIN 
PULSEPERREV 
GEARBOX 
TICKSPERSEC 
TICKSTOMMSEC 
TICKSTORPM 
TicksPerMS
ENC_A_HIGH  
ENC_A_LOW 
ENC_B_HIGH 
ENC_B_LOW 
PWM_A_HIGH 
PWM_A_LOW 
PWM_B_HIGH 
PWM_B_LOW 
MAXDuty 
MINDuty 
TURNBOTHMOTORS 
TURNONEMOTOR 
TurnTime90 
StoppedMotorTime
FORWARD 
BACKWARD 
INACTIVE 
TurnRPM 
StraightRPM 
ControlInit
InputCaptureInit
ControlPinInit
PWMSetup
DisableControl
EnableControl
MoveForward
MoveBackward
RotateCW
StopMovement
CalculateRPM

LastTimerA
CurrentTimerA
PulseWidthA

LastTimerB
CurrentTimerB
PulseWidthB

CurrentRPM_A
TargetRPM_A
RPMError_A
SumError_A
RequestedDuty_A

CurrentRPM_B
TargetRPM_B
RPMError_B
SumError_B
RequestedDuty_B

MotorADirection
MotorBDirection

EncoderCount_A
EncoderCount_B

MotorMovementA
MotorMovementB


bool InitMotorSM(uint8_t Priority)

	//Save priority recieved
  //Call PWMSetup function
  //Enables NVIC for WT0A and WT0B
  //Calls ControlIni
	//Set MotorMovementA and MotorMovementB as NoEdges
  //Post initial transition event


/****************************************************************************
 Function
    RunMotorSM

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes

 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunMotorSM(ES_Event_t ThisEvent)

	//Reads current State
		//If CurrentState is InitMotor
			//If recieved event is ES_INIT
				//Kickoff Wide Timer 0A, 0B, 4A
				//Call Stop Movement Function
				//Set next state to ReadyMotor
		//If CurrentState is ReadyMotor
			//If event recieved is MOTOR_COMMAND
				//If EventParam is FORWARD
					//Call MoveForward function with StraightRPM
					//Set next state to ReadyMotor
				//If EventParam is BACKWARDS
					//Call MoveBackward function with StraightRPM
					//Set next state to ReadyMotor
				//If EventParam is LEFT_STOP_RIGHT_BACK
					//Call RotateCW function with TurnRPM and TURNONEMOTOR parameter
					//Set next state to ReadyMotor
				//If EventParam is RIGHT_STOP_LEFT_BACK
					//Call RotateCCW function with TurnRPM and TURNONEMOTOR parameter
					//Set next state to ReadyMotor
				//If EventParam is CW
					//Call RotateCW function with TurnRPM and TURNBOTHMOTORS parameter
					//Set next state to ReadyMotor
				//If EventParam is CCW
					//Call RotateCCW function with TurnRPM and TURNBOTHMOTORS parameter
					//Set next state to ReadyMotor
				//If EventParam is CW90
					//Call RotateCW function with TurnRPM and TURNBOTHMOTORS parameter
					//Start MotorTimer with TurnTime90 constant
					//Set next state  to RotateCW90
				//If EventParam is CCW90
					//Call RotateCCW function with TurnRPM and TURNBOTHMOTORS parameter
					//Start MotorTimer with TurnTime90 constant
					//Set next state  to RotateCCW90
				//If EventParam is STOP
					//Stop motors
					//Set next state to ReadyMotor
			//If recieved event is ES_TIMEOUT
				//If EventParam is MotorAStoppedTimer
					//Set MotorMovementA to NoEdges
				//If EventParam is MotorBStoppedTimer
					//Set MotorMovementB to NoEdges
		//If CurrentState is RotateCW90
			//If event recieved is ES_TIMEOUT
				//If EventParam is MotorTimer
					//Stop motors
					//Set next state to ReadyMotor
				//If EventParam is MotorAStoppedTimer
					//Set MotorMovementA to NoEdges
				//If EventParam is MotorBStoppedTimer
					//Set MotorMovementB to NoEdges
			//If recieved event is MOTOR_COMMAND and EventParam is STOP
				//Stop motors
				//Set next state to ReadyMotor
		//If CurrentState is RotateCCW90
			//If event recieved is ES_TIMEOUT
				//If EventParam is MotorTimer
					//Stop motors
					//Set next state to ReadyMotor
				//If EventParam is MotorAStoppedTimer
					//Set MotorMovementA to NoEdges
				//If EventParam is MotorBStoppedTimer
					//Set MotorMovementB to NoEdges
			//If recieved event is MOTOR_COMMAND and EventParam is STOP
				//Stop motors
				//Set next state to ReadyMotor
  //return ReturnEvent;

// NOTE: MotorA is LEFT, MotorB is RIGHT
// Movement Functions

static void MoveBackward(uint16_t DesiredRPM)
  //Turn on polarity bits for Motor A and Motor B
  //Set MotorADirection to false
	//Set MotorBDirection to false
  //Set initial duties to Motor A and Motor B
  //Save TargetRPM_A and TargetRPM_B with DesiredRPM

static void MoveForward(uint16_t DesiredRPM)
	//Turn off polarity bits for Motor A and Motor B
  //Set MotorADirection to true
	//Set MotorBDirection to true
  //Set initial duties to Motor A and Motor B
  //Save TargetRPM_A and TargetRPM_B with DesiredRPM

static void StopMovement(void)
  //Turn off polarity bits for Motor A and Motor B
  //Set MotorADirection to true
	//Set MotorBDirection to true
  //Save TargetRPM_A and TargetRPM_B with 0 RPM

static void RotateCCW(uint16_t DesiredRPM, uint8_t NumMotors)
  //LEFT Forward, RIGHT backwards
  //if NumMotors equals TURNBOTHMOTORS
    //Set polarity bits for CCW turn (A low, B high)
    //set direction for CCW turn (A false, B true)
    //Save Desired RPM for TargetRPM_A and TargetRPM_B
	//if NumMotors equals TURNONEMOTOR
		//Set polarity bits for CCW turn (A low, B high)
    //set direction for CCW turn (A false, B true)
    //Save Desired RPM for TargetRPM_A
		//Save 0 to TargetRPM_B

static void RotateCW(uint16_t DesiredRPM, uint8_t NumMotors)


  //RIGHT Forward, LEFT backwards
  //if NumMotors equals TURNBOTHMOTORS
		//Set polarity bits for CW turn (B low, A high)
    //set direction for CW turn (A true, B false)
    //set direction bytes for CW turn
    //Save Desired RPM for TargetRPM_A and TargetRPM_B
  //if NumMotors equals TURNONEMOTOR
		//Set polarity bits for CW turn (B low, A high)
    //set direction for CW turn (A true, B false)
    //Save Desired RPM for TargetRPM_B
		//Save 0 to TargetRPM_A


//Helper Functions
static void ControlInit(void)
  //Timer Initialization
  //Enable clock to Wide timer 4
  //Wait until wide timer 4 has finished initializing
  //Setting up base timer settings
  //Disabling Timer A
  //Set up as 32-bit individual timer
  //Using 32-bit count
  //Setting up Timer 4-A for 2ms rate for control algorithm
  // set up timer A in periodic mode so that it repeats the time-outs
  //Set up for 2ms timeout
  // enable a local timeout interrupt
  // Enable on NVIC (102 for WT4A)
  //Setting lower priority for WT4A
  //The timer will enable on the init function


static void PWMSetup(void)
  // Finish enabling the PWM and loading the frequency value here
  // Set the PWM frequency of 3500 Hz to DriveMotors
  // enable the PWM outputs to DriveMotors (PWM0, enable PWM0 and PWM1)


static uint32_t CalculateRPM(uint8_t Motor)
  //Switch depending on motor
		//If MotorA
			//If MotorMovementA equals GoodEdges
				//Divide TICKSTORPM over PulseWidthA for RPM_A
				//Return RPM_A
			//Else
				//Return 0
		//If MotorB
			//If MotorMovementB equals GoodEdges
				//Divide TICKSTORPM over PulseWidthB for RPM_B
				//Return RPM_B
			//Else
				//Return 0

void CapturePulseA(void)
{
  //Clearing interrupt flag (ISR)
  //Saving last Timer Capture for MotorA in CurrentTimerA
  //Deal with if we can calculate pulse or not
	//If MotorMovementA equals NoEdges
		// we can go into OneEdge, but we can't calculate pulse
	//else if MotorMovementA equals OneEdge
		// we can now calculate pulse, but first go into GoodEdges
		//Calculating Pulse Width by subtracting CurrentTimerA minus LastTimerA
		//Saving history of CurrentTimerA to LastTimerA
	//else 
		//Calculating Pulse Width by subtracting CurrentTimerA minus LastTimerA
		//Saving history of CurrentTimerA to LastTimerA
	//Start MotorAStoppedTimer with StoppedMotorTime constant


void CapturePulseB(void)
	//Clearing interrupt flag (ISR)
  //Saving last Timer Capture for MotorB in CurrentTimerB
  //Deal with if we can calculate pulse or not
	//If MotorMovementB equals NoEdges
		// we can go into OneEdge, but we can't calculate pulse
	//else if MotorMovementB equals OneEdge
		// we can now calculate pulse, but first go into GoodEdges
		//Calculating Pulse Width by subtracting CurrentTimerB minus LastTimerB
		//Saving history of CurrentTimerB to LastTimerB
	//else 
		//Calculating Pulse Width by subtracting CurrentTimerB minus LastTimerB
		//Saving history of CurrentTimerB to LastTimerB
	//Start MotorBStoppedTimer with StoppedMotorTime constant


void MotorControl(void)
  //Clearing interrupt flag (ISR)
  //Call the RPM function for CurrentRPM_A
	//Call the RPM function for CurrentRPM_B
	//Setup RPMError_A as TargetRPM_A - CurrentRPM_A
	//Setup RPMError_B as TargetRPM_B - CurrentRPM_B
	//Sum SumError_A with RPMError_A
	//Sum SumError_B with RPMError_B

  //Calculate RequestedDuty_A as PROP_GAIN * (RPMError_A + (INT_GAIN * SumError_A))
	//Calculate RequestedDuty_B as PROP_GAIN * (RPMError_B + (INT_GAIN * SumError_B))

  //Add antiwindup for Motor A
  //Add antiwindup for Motor B

	//if MotorADirection equals true
		//Set PWM Duty RequestedDuty_A for MotorADirection
	//else
		//Set PWM Duty as 100-RequestedDuty_A for MotorADirection
	
	//if MotorBDirection equals true
		//Set PWM Duty RequestedDuty_B for MotorBDirection
	//else
		//Set PWM Duty as 100-RequestedDuty_B for MotorBDirection