/******************************************************************************** Module MotorService.c Revision 1.0.1 Description This is a template file for implementing a simple service under the Gen2 Events and Services Framework. Notes History When Who What/Why -------------- --- -------- 01/16/12 09:58 jec began conversion from TemplateFSM.c ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ /* include header files for this state machine as well as any machines at the next lower level in the hierarchy that are sub-machines to this machine */ #include "ES_Configure.h" #include "ES_Framework.h" #include "MotorService.h" #include "TopHSM.h" #include #include /*----------------------------- Module Defines ----------------------------*/ #define TICKS_PER_REV 200 #define PWMPERIOD 100 #define _MS_ *375 #define DC_UPDATE_PERIOD (50 _MS_) #define RAMPTIME 100 /*---------------------------- Module Functions ---------------------------*/ /* prototypes for private functions for this service.They should be functions relevant to the behavior of this service */ void interrupt _Vec_tim0ch4 LeftEncoderTimer (void); void interrupt _Vec_tim0ch5 RightEncoderTimer (void); void interrupt _Vec_tim0ch6 DutyCycleUpdate (void); /*---------------------------- Module Variables ---------------------------*/ // with the introduction of Gen2, we need a module level Priority variable static uint8_t MyPriority; static unsigned int ticks; static unsigned int maxTicks; static unsigned int desiredRPMLeft; static unsigned int desiredRPMRight; static unsigned int nTicksLeft; static unsigned int nTicksRight; static bool countingTicks; static bool sensingWall; static int numberLinearTicks; static bool fullSpeed; static bool firstRead; static MotorState_t CurrentState; static long sumErrorLeft; static long sumErrorRight; static long sumSpeedErrorLeft; static long sumSpeedErrorRight; static int nUpdateCycles; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function InitTemplateService 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 InitMotorService ( uint8_t Priority ) { ES_Event ThisEvent; MyPriority = Priority; DDRT |= (BIT2HI | BIT3HI); //we will use these pins to control motor direction DDRT &= (BIT0LO & BIT1LO); //encoders PTT &= (BIT2LO & BIT3LO); //PWM ENABLE FOR MOTORS PWME |= (_S12_PWME4 | _S12_PWME5); //enable 2 PWM channels PWMPRCLK |= 0b00000111; //prescale by 128 PWMCLK |= (_S12_PCLK4 | _S12_PCLK5); //we want to use clock after scaling PWMSCLA = 12; //divide by an additional 30 MODRR |= (_S12_MODRR4 | _S12_MODRR5); //Use U4 and U5 as PWM outputs PWMPOL |= _S12_PPOL4; PWMPER4 = PWMPERIOD; //b/c 24,000,000/128/30/100 = 62.5Hz (max spec of H-bridge) PWMDTY4 = 0; PWMPOL |= _S12_PPOL5; PWMPER5 = PWMPERIOD; PWMDTY5 = 0; TIM0_TSCR1 = _S12_TEN; //enable timers TIM0_TSCR2 = _S12_PR2 | _S12_PR1;//prescale to 64 TIM0_TIOS &= ~_S12_IOS4;//encoder input capture TIM0_TIOS &= ~_S12_IOS5;//encoder input capture TIM0_TIOS |= _S12_IOS6;//dc update output compare TIM0_TFLG1 = _S12_C4F; TIM0_TFLG1 = _S12_C5F; TIM0_TFLG1 = _S12_C6F; TIM0_TIE |= _S12_C4I; TIM0_TIE |= _S12_C5I; TIM0_TIE |= _S12_C6I; TIM0_TCTL3 |= _S12_EDG4A; TIM0_TCTL3 |= _S12_EDG5A; TIM0_TC6 = TIM0_TCNT + DC_UPDATE_PERIOD;//set for time in the future EnableInterrupts; sumErrorLeft = 0.0; sumErrorRight = 0.0; sumSpeedErrorLeft = 0.0; sumSpeedErrorRight = 0.0; nUpdateCycles = 0; ticks = 0; maxTicks = 0; countingTicks = false; fullSpeed = false; sensingWall = false; firstRead = true; CurrentState = Stopped; ThisEvent.EventType = ES_INIT; if (ES_PostToService( MyPriority, ThisEvent) == true) { return true; } else { return false; } } /**************************************************************************** Function PostTemplateService 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 PostMotorService( ES_Event ThisEvent ) { return ES_PostToService( MyPriority, ThisEvent); } /**************************************************************************** Function RunTemplateService 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 RunMotorService( ES_Event ThisEvent ) { ES_Event ReturnEvent; ReturnEvent.EventType = ES_NO_EVENT; // assume no errors if(ThisEvent.EventType == ES_TIMEOUT) { printf("TimeOut"); if (!fullSpeed) { desiredRPMLeft = desiredRPMLeft+2; desiredRPMRight = desiredRPMLeft+2; if(desiredRPMLeft < FULL_SPEED_RPM) { ES_Timer_InitTimer(MOTOR_TIMER, RAMPTIME); } else { desiredRPMLeft = FULL_SPEED_RPM; desiredRPMRight = FULL_SPEED_RPM; fullSpeed = true; } } else { desiredRPMLeft = desiredRPMLeft-5; desiredRPMRight = desiredRPMLeft-5; printf("SlowDown"); if(desiredRPMLeft > HALF_SPEED_RPM) { ES_Timer_InitTimer(MOTOR_TIMER, RAMPTIME); } else { desiredRPMLeft = HALF_SPEED_RPM; desiredRPMRight = HALF_SPEED_RPM; fullSpeed = false; } } } if (ThisEvent.EventType == ES_MOTOR) { sumErrorLeft = 0.0; sumErrorRight = 0.0; sumSpeedErrorLeft = 0.0; sumSpeedErrorRight = 0.0; firstRead = true; nUpdateCycles = 0; printf("Motor Call. Type: %d\n", ThisEvent.EventParam); switch ( ThisEvent.EventParam ) { case 0x00: //stop PWMDTY4 = 0; PWMDTY5 = 0; PWMPOL |= _S12_PPOL4; PWMPOL |= _S12_PPOL5; CurrentState = Stopped; countingTicks = false; ticks = 0; PTT &= (BIT2LO & BIT3LO); desiredRPMLeft = 0; desiredRPMRight = 0; break; case 0x0B: //counterclockwise CurrentState = CounterclockwiseIndefinite; countingTicks = false; PWMPOL |= _S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT |= BIT2HI; PTT &= BIT3LO; //ES_Timer_InitTimer(6, 2000); //ES_Timer_StartTimer(6); break; case 0x01: //counterclockwise 90 CurrentState = Counterclockwise90; countingTicks = true; maxTicks = TURN_NINETY_TICKS; PWMPOL |= _S12_PPOL5; PWMPOL &= ~_S12_PPOL4; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT |= BIT3HI; PTT &= BIT2LO; //ES_Timer_InitTimer(6, 2000); //ES_Timer_StartTimer(6); break; case 0x02: //clockwise 90 CurrentState = Clockwise90; countingTicks = true; maxTicks = TURN_NINETY_TICKS; PWMPOL |= _S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT &= BIT3LO; PTT |= BIT2HI; //ES_Timer_InitTimer(6, 2000); //ES_Timer_StartTimer(6); break; case 0x03: //forward half CurrentState = ForwardHalf; PWMPOL |= _S12_PPOL4; PWMPOL |= _S12_PPOL5; desiredRPMLeft = HALF_SPEED_RPM; desiredRPMRight = HALF_SPEED_RPM; PTT &= BIT2LO; PTT &= BIT3LO; break; case 0x04: //forward full; CurrentState = ForwardFull; PWMPOL |= _S12_PPOL4; PWMPOL |= _S12_PPOL5; desiredRPMLeft = HALF_SPEED_RPM; desiredRPMRight = HALF_SPEED_RPM; ES_Timer_InitTimer(MOTOR_TIMER, RAMPTIME); PTT &= BIT2LO; PTT &= BIT3LO; sensingWall = true; break; case 0x0D: //forward Jousting; CurrentState = ForwardJoust; PWMPOL |= _S12_PPOL4; PWMPOL |= _S12_PPOL5; desiredRPMLeft = JOUST_RPM; desiredRPMRight = JOUST_RPM; PTT &= BIT2LO; PTT &= BIT3LO; sensingWall = true; break; case 0x05: //backward half CurrentState = BackwardHalf; PWMPOL &= ~_S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = HALF_SPEED_RPM; desiredRPMRight = HALF_SPEED_RPM; PTT |= BIT2HI; PTT |= BIT3HI; break; case 0x06: //backward full CurrentState = BackwardFull; PWMPOL &= ~_S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = HALF_SPEED_RPM; desiredRPMRight = HALF_SPEED_RPM; ES_Timer_InitTimer(MOTOR_TIMER, RAMPTIME); PTT |= BIT2HI; PTT |= BIT3HI; sensingWall = true; break; case 0x0E: //backward joust CurrentState = BackwardJoust; PWMPOL &= ~_S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = JOUST_RPM; desiredRPMRight = JOUST_RPM; PTT |= BIT2HI; PTT |= BIT3HI; sensingWall = true; break; case 0x07: //forward short; CurrentState = ForwardShort; countingTicks = true; maxTicks = SHORT_TICKS; PWMPOL |= _S12_PPOL4; PWMPOL |= _S12_PPOL5; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT &= BIT2LO; PTT &= BIT3LO; break; case 0x08: //forward long; CurrentState = ForwardLong; countingTicks = true; maxTicks = LONG_TICKS; PWMPOL |= _S12_PPOL4; PWMPOL |= _S12_PPOL5; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT &= BIT2LO; PTT &= BIT3LO; break; case 0x09: //backward short CurrentState = BackwardShort; countingTicks = true; maxTicks = SHORT_TICKS; PWMPOL &= ~_S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT |= BIT2HI; PTT |= BIT3HI; break; case 0x0A: //backward long CurrentState = BackwardLong; countingTicks = true; maxTicks = LONG_TICKS; PWMPOL &= ~_S12_PPOL4; PWMPOL &= ~_S12_PPOL5; desiredRPMLeft = DEFINITE_DISTANCE_RPM; desiredRPMRight = DEFINITE_DISTANCE_RPM; PTT |= BIT2HI; PTT |= BIT3HI; break; } } return ReturnEvent; } MotorState_t QueryMotorService ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ void interrupt _Vec_tim0ch4 LeftEncoderTimer (void) { static ES_Event MotorResponse; MotorResponse.EventType = ES_MOTOR_RESPONSE; nTicksLeft++; if (countingTicks) { ticks++; if (ticks >= maxTicks){ PWMDTY4 = 0; PWMDTY5 = 0; CurrentState = Stopped; countingTicks = false; ticks = 0; PTT &= (BIT4LO & BIT5LO); desiredRPMLeft = 0; desiredRPMRight = 0; PostMasterSM(MotorResponse); printf("Ended Turn"); } } TIM0_TFLG1 = _S12_C4F; } void interrupt _Vec_tim0ch5 RightEncoderTimer (void) { nTicksRight++; if (sensingWall) { numberLinearTicks++; if (numberLinearTicks >= fieldDistanceTicks){ sensingWall = false; if (CurrentState == ForwardFull) { printf("Time To Slow Down\n"); ES_Timer_InitTimer(MOTOR_TIMER, RAMPTIME); } else if (CurrentState == BackwardFull) { ES_Timer_InitTimer(MOTOR_TIMER, RAMPTIME); } } } TIM0_TFLG1 = _S12_C5F; } void interrupt _Vec_tim0ch6 DutyCycleUpdate (void) { static ES_Event MotorResponse; static int nCycles; static int RPMErrorLeft; static float dutyCycleLeft; static int RPMErrorRight; static float dutyCycleRight; static float differentialLeft; static float differentialRight; MotorResponse.EventType = ES_MOTOR_RESPONSE; TIM0_TC6 += DC_UPDATE_PERIOD ; TIM0_TFLG1 = _S12_C6F; nUpdateCycles++; if (firstRead == true) { nTicksLeft = 0; nTicksRight = 0; firstRead = false; } if (CurrentState != Stopped) { // printf("NTICKSLEFT: %d NTICKSRIGHT: %d\n\r", nTicksLeft, nTicksRight); RPMErrorLeft = nTicksLeft - desiredRPMLeft; RPMErrorRight = nTicksRight - desiredRPMRight; dutyCycleLeft = (int) (-Kp*RPMErrorLeft - Ki*sumErrorLeft); dutyCycleRight = (int) (-Kp*RPMErrorRight - Ki*sumErrorRight); // printf("Left: %f Right %f\n\r", dutyCycleLeft, dutyCycleRight); sumErrorRight += RPMErrorRight; sumErrorLeft += RPMErrorLeft; // differentialLeft = nTicksLeft- nTicksRight; // differentialRight = nTicksRight - nTicksLeft; // dutyCycleLeft = dutyCycleLeft - .1*differentialLeft; // dutyCycleRight = dutyCycleRight - .1*differentialRight; EnableInterrupts; nTicksLeft = 0; nTicksRight =0; // printf("Left: %f Right %f\n\r", dutyCycleLeft, dutyCycleRight); if( (dutyCycleLeft >= 100) && (dutyCycleRight >= 100) && (!countingTicks) && (!fullSpeed) && (nUpdateCycles > 10) ) { sensingWall = false; PostMasterSM(MotorResponse); MotorResponse.EventType = ES_MOTOR; MotorResponse.EventParam = 0x00; PostMotorService(MotorResponse); numberLinearTicks = 0; printf("Hit Wall"); } if (dutyCycleLeft > 100) { dutyCycleLeft = 100; sumErrorLeft -= RPMErrorLeft; sumSpeedErrorLeft -= differentialLeft; } else if (dutyCycleLeft < 0) { dutyCycleLeft = 0; sumErrorLeft += RPMErrorLeft; sumSpeedErrorLeft += differentialLeft; } PWMDTY4 = (char) dutyCycleLeft; if (dutyCycleRight > 100) { dutyCycleRight = 100; sumErrorRight -= RPMErrorRight; sumSpeedErrorRight -= differentialRight; } else if (dutyCycleRight < 0) { dutyCycleRight = 0; sumErrorRight += RPMErrorRight; sumSpeedErrorRight += differentialRight; } if (nCycles%2 == 0) { if (CurrentState == ForwardFull || CurrentState == ForwardJoust ) { PWMDTY5 = (char) dutyCycleRight - 6; } else { PWMDTY5 = (char) dutyCycleRight - 4; } } else { if (CurrentState == ForwardFull || CurrentState == ForwardJoust) { PWMDTY5 = (char) dutyCycleRight - 5; } else { PWMDTY5 = (char) dutyCycleRight - 3; } } nCycles++; PWMDTY5 = (char) dutyCycleRight; } }