/**************************************************************************** Module MotorService.c Revision 1.0.1 Description This is the first service for the Test Harness under the Gen2 Events and Services Framework. Notes History When Who What/Why -------------- --- -------- 10/26/17 18:26 jec moves definition of ALL_BITS to ES_Port.h 10/19/17 21:28 jec meaningless change to test updating 10/19/17 18:42 jec removed referennces to driverlib and programmed the ports directly 08/21/17 21:44 jec modified LED blink routine to only modify bit 3 so that I can test the new new framework debugging lines on PF1-2 08/16/17 14:13 jec corrected ONE_SEC constant to match Tiva tick rate 11/02/13 17:21 jec added exercise of the event deferral/recall module 08/05/13 20:33 jec converted to test harness service 01/16/12 09:58 jec began conversion from TemplateFSM.c ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ // TODO: Double check initialiation works // TODO: define distance // This module #include "MotorSM.h" // Hardware #include "inc/hw_memmap.h" #include "inc/hw_types.h" #include "inc/hw_gpio.h" #include "inc/hw_sysctl.h" #include "inc/hw_nvic.h" #include "inc/hw_pwm.h" #include "inc/hw_timer.h" #include "driverlib/gpio.h" #include "driverlib/sysctl.h" #include "driverlib/pin_map.h" #include "driverlib/pwm.h" // Event & Services Framework #include "ES_Configure.h" #include "ES_Framework.h" #include "ES_DeferRecall.h" #include "ES_ShortTimer.h" #include "ES_Port.h" //Include other files #include "PWM_Module.h" #include "GameplayHSM.h" //#include "SPIService.h" #include "HardwareInitialize.h" /*----------------------------- Module Defines ----------------------------*/ /// these times assume a 1.000mS/tick timing #define ONE_SEC 1000 #define HALF_SEC (ONE_SEC / 2) #define TWO_SEC (ONE_SEC * 2) #define FIVE_SEC (ONE_SEC * 5) //Control Law defines #define PROP_GAIN 2.3 #define INT_GAIN 0.1 //RPM Constants #define PULSEPERREV 12 #define GEARBOX 50 #define TICKSPERSEC 40000000 #define TICKSTOMMSEC 20733333 #define TICKSTORPM 4000000 //Ticks per MS #define TicksPerMS 40000 //Define Constants for Bit names #define ENC_A_HIGH BIT4HI #define ENC_A_LOW BIT4LO #define ENC_B_HIGH BIT5HI #define ENC_B_LOW BIT4LO #define PWM_A_HIGH BIT0HI #define PWM_A_LOW BIT0LO #define PWM_B_HIGH BIT1HI #define PWM_B_LOW BIT1LO // PWM Defines #define MAXDuty 100 #define MINDuty 0 // RPM Defines #define TURNBOTHMOTORS 2 #define TURNONEMOTOR 1 #define TurnTime90 2200 //TODO- empirical #define StoppedMotorTime 100 //Motor Conditions #define FORWARD 0 #define BACKWARD 1 #define INACTIVE 2 // define speeds #define TurnRPM 4 #define StraightRPM 9 /*---------------------------- Module Functions ---------------------------*/ /* prototypes for private functions for this service.They should be functions relevant to the behavior of this service */ //Initialization functions static void ControlInit(void); static void InputCaptureInit(void); static void ControlPinInit(void); static void PWMSetup(void); //PI control interrupt enable static void DisableControl(void); static void EnableControl(void); //Movement Functions static void MoveForward(uint16_t DesiredRPM); static void MoveBackward(uint16_t DesiredRPM); static void RotateCW(uint16_t DesiredRPM, uint8_t NumMotors); static void RotateCCW(uint16_t DesiredRPM, uint8_t NumMotors); static void StopMovement(void); //Other functions static uint32_t CalculateRPM(uint8_t Motor); /*---------------------------- Module Variables ---------------------------*/ // with the introduction of Gen2, we need a module level Priority variable static uint8_t MyPriority; static MotorState_t CurrentState; //InputCapture variables static volatile uint32_t LastTimerA; static volatile uint32_t CurrentTimerA = 0; static volatile uint32_t PulseWidthA; static volatile uint32_t LastTimerB; static volatile uint32_t CurrentTimerB = 0; static volatile uint32_t PulseWidthB; //Defining control variables static uint32_t CurrentRPM_A; static uint32_t TargetRPM_A; static int32_t RPMError_A; static int32_t SumError_A; static int32_t RequestedDuty_A; static uint32_t CurrentRPM_B; static uint32_t TargetRPM_B; static int32_t RPMError_B; static int32_t SumError_B; static int32_t RequestedDuty_B; //Defining boolean direction pins static bool MotorADirection = true; static bool MotorBDirection = true; //Defining Encoder Counts static uint32_t EncoderCount_A; static uint32_t EncoderCount_B; ////Defining Initial RPM values //static uint8_t TurnRPM=5; //static uint8_t StraightRPM=13; //Defining names for if we can calculate pulse width or not typedef enum { GoodEdges, NoEdges, OneEdge } MotorMovementState; static MotorMovementState MotorMovementA; static MotorMovementState MotorMovementB; // add a deferral queue for up to 3 pending deferrals +1 to allow for ovehead //static ES_Event_t DeferralQueue[3 + 1]; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function InitMotorSM Parameters uint8_t : the priorty of this service Returns bool, false if error in initialization, true otherwise Description Saves away the priority, and does any other required initialization for this service Notes Author J. Edward Carryer, 01/16/12, 10:00 ****************************************************************************/ bool InitMotorSM(uint8_t Priority) { ES_Event_t ThisEvent; MyPriority = Priority; /******************************************** in here you write your initialization code *******************************************/ //Initialize hardware using function InitializeHardware(); //Initialize functions, to be replaced by master init function //Initializing PWM // PWM_Init(2); // PWM_SetFrequency(3500,0); // ControlPinInit(); //Finish setting up PWM PWMSetup(); //Enables NVIC for WT0A and WT0B HWREG(NVIC_EN2) |= (BIT30HI | BIT31HI); //Calls Initialization of Wide Timer for control ControlInit(); MotorMovementA = NoEdges; MotorMovementB = NoEdges; //CurrentState=ReadyMotor; //Debugging aid // post the initial transition event ThisEvent.EventType = ES_INIT; CurrentState = InitMotor; if (ES_PostToService(MyPriority, ThisEvent) == true) { return true; } else { return false; } } /**************************************************************************** Function PostMotorSM Parameters EF_Event ThisEvent ,the event to post to the queue Returns bool false if the Enqueue operation failed, true otherwise Description Posts an event to this state machine's queue Notes Author J. Edward Carryer, 10/23/11, 19:25 ****************************************************************************/ bool PostMotorSM(ES_Event_t ThisEvent) { return ES_PostToService(MyPriority, ThisEvent); } /**************************************************************************** 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) { ES_Event_t ReturnEvent; ReturnEvent.EventType = ES_NO_EVENT; // assume no errors //Pass NextState to CurrentState switch (CurrentState) { case InitMotor: { if (ThisEvent.EventType == ES_INIT) { // //Enable Global Interrupts // __enable_irq(); //Kick off WT4A, WT0 timers A and B HWREG(WTIMER0_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TBEN | TIMER_CTL_TASTALL | TIMER_CTL_TBSTALL); HWREG(WTIMER4_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL); //start with motors stopped StopMovement(); //Transition to next state CurrentState = ReadyMotor; } } break; case ReadyMotor: { switch (ThisEvent.EventType) { case MOTOR_COMMAND: { switch (ThisEvent.EventParam) { case FORWARD: { //Call function to move forward MoveForward(StraightRPM); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; case BACKWARDS: { //Call function to move backwards MoveBackward(StraightRPM); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; case LEFT_STOP_RIGHT_BACK: { //Turn right using left wheel as pivot RotateCW(TurnRPM, TURNONEMOTOR); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; case RIGHT_STOP_LEFT_BACK: { //Turn left using right wheel as pivot RotateCCW(TurnRPM, TURNONEMOTOR); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; case CW: { //Rotate CW on own axis RotateCW(TurnRPM, TURNBOTHMOTORS); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; case CCW: { //Rotate CCW on own axis RotateCCW(TurnRPM, TURNBOTHMOTORS); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; case CW90: { //Rotate CW on own axis for 90 deg RotateCW(TurnRPM, TURNBOTHMOTORS); //Initialize 90deg turn timer ES_Timer_InitTimer(MotorTimer, TurnTime90); //Change to turning CW 90 deg state CurrentState = RotateCW90; } break; case CCW90: { //Rotate CCW on own axis for 90 deg RotateCCW(TurnRPM, TURNBOTHMOTORS); //Initialize 90deg turn timer ES_Timer_InitTimer(MotorTimer, TurnTime90); //Change to turning CW 90 deg state CurrentState = RotateCCW90; } break; case STOP: { //Stop motors StopMovement(); //Stay in Ready Motor state CurrentState = ReadyMotor; } break; } } break; case ES_TIMEOUT: { // check which timer it is if (ThisEvent.EventParam == MotorAStoppedTimer) { // motor A has stopped, set it to NoEdges MotorMovementA = NoEdges; } else if (ThisEvent.EventParam == MotorBStoppedTimer) { // motor B has stopped, set it to NoEdges MotorMovementB = NoEdges; } } break; } } break; case RotateCW90: { if (ThisEvent.EventType == ES_TIMEOUT) { // check which timer it is if (ThisEvent.EventParam == MotorTimer) { //Stop rotating StopMovement(); //Return to ready motor state CurrentState = ReadyMotor; } else if (ThisEvent.EventParam == MotorAStoppedTimer) { // motor A has stopped, set it to NoEdges MotorMovementA = NoEdges; } else if (ThisEvent.EventParam == MotorBStoppedTimer) { // motor B has stopped, set it to NoEdges MotorMovementB = NoEdges; } } else if ((ThisEvent.EventType == MOTOR_COMMAND) && (ThisEvent.EventParam == STOP)) { //Stop rotating StopMovement(); //Return to ready motor state CurrentState = ReadyMotor; } } break; case RotateCCW90: { if (ThisEvent.EventType == ES_TIMEOUT) { // check which timer it is if (ThisEvent.EventParam == MotorTimer) { //Stop rotating StopMovement(); //Return to ready motor state CurrentState = ReadyMotor; } else if (ThisEvent.EventParam == MotorAStoppedTimer) { // motor A has stopped, set it to NoEdges MotorMovementA = NoEdges; } else if (ThisEvent.EventParam == MotorBStoppedTimer) { // motor B has stopped, set it to NoEdges MotorMovementB = NoEdges; } } else if ((ThisEvent.EventType == MOTOR_COMMAND) && (ThisEvent.EventParam == STOP)) { //Stop rotating StopMovement(); //Return to ready motor state CurrentState = ReadyMotor; } } } return ReturnEvent; } ///*************************************************************************** // Setter functions // ***************************************************************************/ //void SetNewStraightRPM (uint8_t NewRPM){ // StraightRPM=NewRPM; //} //void SetNewTurnRPM (uint8_t NewRPM){ // TurnRPM=NewRPM; //} /*************************************************************************** private functions ***************************************************************************/ // NOTE: MotorA is LEFT, MotorB is RIGHT // Movement Functions static void MoveBackward(uint16_t DesiredRPM) { //Enable control EnableControl(); //Turn on polarity bits HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= (PWM_B_HIGH | PWM_A_HIGH); //Set direction backwards MotorADirection = false; MotorBDirection = false; //Set initial duties to get the motors started PWM_SetDuty(50, MotorA); PWM_SetDuty(50, MotorB); //Set RPM Target TargetRPM_B = DesiredRPM; TargetRPM_A = DesiredRPM; } static void MoveForward(uint16_t DesiredRPM) { //Enable control EnableControl(); //Turn off polarity bits //HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= (PWM_B_HIGH); HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_B_LOW & PWM_A_LOW); //Set direction forward MotorADirection = true; MotorBDirection = true; //Set initial duties to get the motors started PWM_SetDuty(50, MotorA); PWM_SetDuty(50, MotorB); //Set RPM Target TargetRPM_B = DesiredRPM; TargetRPM_A = DesiredRPM; } static void StopMovement(void) { //Set Polarity bits low HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_B_LOW & PWM_A_LOW); //Set direction forward MotorADirection = true; MotorBDirection = true; //Set RPM Target TargetRPM_B = 0; TargetRPM_A = 0; } static void RotateCCW(uint16_t DesiredRPM, uint8_t NumMotors) { //RIGHT Stop, LEFT backwards //Take in the number of motors to turn and set desiredRPM //LEFT Forward, RIGHT backwards if (NumMotors == TURNBOTHMOTORS) { //Disable Control law DisableControl(); //Set polarity bits for CCW turn HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_A_LOW); //backwards HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= (PWM_B_HIGH); //forward //set direction for CCW turn MotorADirection = false; MotorBDirection = true; //Set RPM Target TargetRPM_B = DesiredRPM; TargetRPM_A = DesiredRPM; EnableControl(); } else if (NumMotors == TURNONEMOTOR) { //Disable Control law DisableControl(); //Set polarity bit HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_A_LOW); //backwards HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= (PWM_B_HIGH); //forward //set direction MotorADirection = false; MotorBDirection = true; //Set RPM Target TargetRPM_B = 0; TargetRPM_A = DesiredRPM; EnableControl(); } else { puts("Incorrect NumMotors to Turn: MotorSM FSM: Rotate CCW"); } } static void RotateCW(uint16_t DesiredRPM, uint8_t NumMotors) { //Take in the number of motors to turn and set desiredRPM //RIGHT Forward, LEFT backwards if (NumMotors == TURNBOTHMOTORS) { //Disable Control law DisableControl(); //Set Polarity bits for CW turn HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= (PWM_A_HIGH); //forward HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_B_LOW); //backwards //set direction bytes for CW turn MotorADirection = true; MotorBDirection = false; //Set RPM Target TargetRPM_B = DesiredRPM; TargetRPM_A = DesiredRPM; EnableControl(); } else if (NumMotors == TURNONEMOTOR) { //Disable Control law DisableControl(); //Set Polarity bits for CW turn HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_B_LOW); //backwards HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= (PWM_A_HIGH); //forward //set direction MotorADirection = true; MotorBDirection = false; //Set RPM Target TargetRPM_B = DesiredRPM; TargetRPM_A = 0; //Send Push for the motor EnableControl(); } else { puts("Incorrect NumMotors to Turn: MotorSM FSM: Rotate CW"); } } //Helper Functions static void ControlInit(void) { //Timer Initialization //Enable clock to Wide timer 4 HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R4; //Wait until wide timer 4 has finished initializing while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R4) != SYSCTL_PRWTIMER_R4) {} //Setting up base timer settings //Disabling Timer A HWREG(WTIMER4_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN; //Set up as 32-bit individual timer HWREG(WTIMER4_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT; //Using 32-bit count HWREG(WTIMER4_BASE + TIMER_O_TAILR) = 0xffffffff; //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 HWREG(WTIMER4_BASE + TIMER_O_TAMR) = (HWREG(WTIMER4_BASE + TIMER_O_TAMR) & ~TIMER_TAMR_TAMR_M) | TIMER_TAMR_TAMR_PERIOD; //Set up for 2ms timeout HWREG(WTIMER4_BASE + TIMER_O_TAILR) = TicksPerMS * 2; // enable a local timeout interrupt HWREG(WTIMER4_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM; // Enable on NVIC (102 for WT4A) HWREG(NVIC_EN3) |= (BIT6HI); //Setting lower priority for WT4A HWREG(NVIC_PRI25) = (HWREG(NVIC_PRI25) & ~NVIC_PRI25_INTC_M) | (1 << NVIC_PRI25_INTC_S); //The timer will enable on the init function } static void InputCaptureInit(void) { //Timer Initialization //Enable clock to Wide timer 0 HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R0; //Enable Clock to Port C HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2; //Wait until wide timer 0 has finished initializing while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R0) != SYSCTL_PRWTIMER_R0) {} //Disabling Timer A HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN; //Disabling Timer B HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEN; //Set up as 32-bit individual timer HWREG(WTIMER0_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT; //Using 32-bit count HWREG(WTIMER0_BASE + TIMER_O_TAILR) = 0xffffffff; HWREG(WTIMER0_BASE + TIMER_O_TBILR) = 0xffffffff; //Setting up Timer A and B for Input capture mode //Set up timer A in capture mode, for edge time and upcounting HWREG(WTIMER0_BASE + TIMER_O_TAMR) = (HWREG(WTIMER0_BASE + TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP); //Set up timer B in capture mode, for edge time and upcounting HWREG(WTIMER0_BASE + TIMER_O_TBMR) = (HWREG(WTIMER0_BASE + TIMER_O_TBMR) & ~TIMER_TBMR_TBAMS) | (TIMER_TBMR_TBCDIR | TIMER_TBMR_TBCMR | TIMER_TBMR_TBMR_CAP); //Saving only rising edge for A HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M; HWREG(WTIMER0_BASE + TIMER_O_CTL) |= TIMER_CTL_TAEVENT_POS; //Saving only rising edge for B HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEVENT_M; HWREG(WTIMER0_BASE + TIMER_O_CTL) |= TIMER_CTL_TBEVENT_POS; //Activating alternate function for port C Bit 4 and 5 HWREG(GPIO_PORTC_BASE + GPIO_O_AFSEL) |= (ENC_A_HIGH | ENC_B_HIGH); //Mapping alt. function 7 for Port C HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) & 0xff00ffff) + (7 << 16) + (7 << 20); //Enable pin 4 on port C for digital I/O HWREG(GPIO_PORTC_BASE + GPIO_O_DEN) |= (ENC_A_HIGH | ENC_B_HIGH); // make pin 4 on Port C into input HWREG(GPIO_PORTC_BASE + GPIO_O_DIR) &= (ENC_A_LOW & ENC_B_LOW); // Enable a local interrupt on the timer HWREG(WTIMER0_BASE + TIMER_O_IMR) |= (TIMER_IMR_CBEIM | TIMER_IMR_CAEIM); // Write on NVIC (94 for WT0A, 95 for WT0B) } static void PWMSetup(void) { // Finish enabling the PWM and loading the frequency value here // Set the PWM period. Since we are counting both up & down, we // initialize the load register to 1/2 the desired total period. We // will also program the match compare registers to 1/2 the desired // high time PWM_SetFrequency(3500, DriveMotors); // // Set the initial Duty cycle on A to 50% by programming the compare // // value to 1/2 the period to count up (or down). Technically, the // // value to program should be Period/2 - DesiredHighTime/2, but since // // the desired high time is 1/2 the period, we can skip the subtract // Duty_0 = 50; // HWREG(PWM0_BASE + PWM_O_0_CMPA) = HWREG(PWM0_BASE + PWM_O_0_LOAD) >> 1; // // Set the initial Duty cycle on B to 50% like A // HWREG(PWM0_BASE + PWM_O_0_CMPB) = HWREG(PWM0_BASE + PWM_O_0_LOAD) >> 1; // enable the PWM outputs HWREG(PWM0_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM0EN | PWM_ENABLE_PWM1EN); } static uint32_t CalculateRPM(uint8_t Motor) { //Declare Static Variables uint32_t RPM_A; uint32_t RPM_B; //Switch case statement switch (Motor) { case MotorA: { if (MotorMovementA == GoodEdges) { //Calculate RPM for A motor RPM_A = (TICKSTORPM) / (PulseWidthA); //printf("RPM Value %d\n\r",RPM_A); return RPM_A; } else { return 0; } } case MotorB: { if (MotorMovementB == GoodEdges) { //Calculate RPM for B motor RPM_B = (TICKSTORPM) / (PulseWidthB); //printf("%d\n\r",RPM_B); return RPM_B; } else { return 0; } } } return 0; } static void ControlPinInit(void) { // enable the clock to Port B HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1; // make sure that Port B is initialized while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R1) != BIT1HI) {} // // Enable pins for digital I/O HWREG(GPIO_PORTB_BASE + GPIO_O_DEN) |= (PWM_A_HIGH | PWM_B_HIGH); // make pin 2 on Port B into outputs HWREG(GPIO_PORTB_BASE + GPIO_O_DIR) |= (PWM_A_HIGH | PWM_B_HIGH); // start with the lines low HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (PWM_A_LOW & PWM_B_LOW); } static void DisableControl(void) { // Disable a local timeout interrupt HWREG(WTIMER4_BASE + TIMER_O_CTL) &= (~TIMER_CTL_TAEN & ~TIMER_CTL_TASTALL); HWREG(WTIMER4_BASE + TIMER_O_IMR) &= ~TIMER_IMR_TATOIM; } static void EnableControl(void) { // enable a local timeout interrupt HWREG(WTIMER4_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL); HWREG(WTIMER4_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM; } /*************************************************************************** ISRs ***************************************************************************/ void CapturePulseA(void) { //Clearing interrupt flag HWREG(WTIMER0_BASE + TIMER_O_ICR) = TIMER_ICR_CAECINT; //Saving last Timer Capture CurrentTimerA = HWREG(WTIMER0_BASE + TIMER_O_TAR); //Deal with if we can calculate pulse or not if (MotorMovementA == NoEdges) { // we can go into OneEdge, but we can't calculate pulse MotorMovementA = OneEdge; LastTimerA = CurrentTimerA; } else if (MotorMovementA == OneEdge) { // we can now calculate pulse, but first go into GoodEdges MotorMovementA = GoodEdges; //Right Motor Encode //Calculating Pulse Width PulseWidthA = CurrentTimerA - LastTimerA; //printf("PWA: %d\n\r",PulseWidthA); //Saving pulse value recieved LastTimerA = CurrentTimerA; //Increasing by 1 encoder Count EncoderCount_A++; } else // otherwise, MotorMovementA is in GoodEdges { //Right Motor Encode //Calculating Pulse Width PulseWidthA = CurrentTimerA - LastTimerA; //printf("PWA: %d\n\r",PulseWidthA); //Saving pulse value recieved LastTimerA = CurrentTimerA; //Increasing by 1 encoder Count EncoderCount_A++; } // start a new timer ES_Timer_InitTimer(MotorAStoppedTimer, StoppedMotorTime); } void CapturePulseB(void) { //Clearing interrupt flag HWREG(WTIMER0_BASE + TIMER_O_ICR) = TIMER_ICR_CBECINT; //Saving last Timer Capture CurrentTimerB = HWREG(WTIMER0_BASE + TIMER_O_TBR); //Deal with if we can calculate pulse or not if (MotorMovementB == NoEdges) { // we can go into OneEdge, but we can't calculate pulse MotorMovementB = OneEdge; LastTimerB = CurrentTimerB; } else if (MotorMovementB == OneEdge) { // we can now calculate pulse, but first go into GoodEdges MotorMovementB = GoodEdges; //Left Motor Encoder //Calculating Pulse Width PulseWidthB = CurrentTimerB - LastTimerB; //Saving pulse value recieved LastTimerB = CurrentTimerB; //Increasing by 1 encoder Count EncoderCount_B++; } else // otherwise, MotorMovementB is in GoodEdges { //Left Motor Encoder //Calculating Pulse Width PulseWidthB = CurrentTimerB - LastTimerB; //Saving pulse value recieved LastTimerB = CurrentTimerB; //Increasing by 1 encoder Count EncoderCount_B++; } // start a new timer ES_Timer_InitTimer(MotorBStoppedTimer, StoppedMotorTime); } void MotorControl(void) { //Clearing interrupt flag HWREG(WTIMER4_BASE + TIMER_O_ICR) = TIMER_ICR_TATOCINT; //Call the RPM function CurrentRPM_A = CalculateRPM(MotorA); CurrentRPM_B = CalculateRPM(MotorB); //Setting RPM error and sum of error RPMError_A = TargetRPM_A - CurrentRPM_A; RPMError_B = TargetRPM_B - CurrentRPM_B; SumError_A += RPMError_A; SumError_B += RPMError_B; //Calculate requested duty RequestedDuty_A = (float)PROP_GAIN * (RPMError_A + (INT_GAIN * SumError_A)); RequestedDuty_B = (float)PROP_GAIN * (RPMError_B + (INT_GAIN * SumError_B)); //Adding antiwindup for Motor A if (RequestedDuty_A > MAXDuty) { RequestedDuty_A = MAXDuty; SumError_A -= RPMError_A; } else if (RequestedDuty_A < MINDuty) { RequestedDuty_A = MINDuty; SumError_A -= RPMError_A; } //Adding antiwindup for Motor B if (RequestedDuty_B > MAXDuty) { RequestedDuty_B = MAXDuty; SumError_B -= RPMError_B; } else if (RequestedDuty_B < MINDuty) { RequestedDuty_B = MINDuty; SumError_B -= RPMError_B; } //Send new value for Motor A if (MotorADirection == true) { PWM_SetDuty(RequestedDuty_A, MotorA); } else { PWM_SetDuty(MAXDuty - RequestedDuty_A, MotorA); } //Send new value for Motor B if (MotorBDirection == true) { PWM_SetDuty(RequestedDuty_B, MotorB); } else { PWM_SetDuty(MAXDuty - RequestedDuty_B, MotorB); } } /*------------------------------- Footnotes -------------------------------*/ /*------------------------------ End of file ------------------------------*/