/**************************************************************************** Module SMDrivetrain.c Description Handles drive motor commands and output, including beacon alignment and tracking Notes In general, "Power" is different from duty cycle, in that the input "power" could be scaled, offset, etc. to maintain a consistent output speed as the batteries die. Power typically ranges from -100 (reverse) to +100 (forward), while duty cycle (0-255) and motor direction (0/1) are calculated and set separately. Direction scales from +100 (left, CCW in place) to 0 (straight) to -100 (right, CW in place). Intermediate values represent driving in an arc, with a more extreme direction being a tighter radius. The only permissible drive actions taken by external functions are timed (except Drive_Stop). This prevents a drive event from being initiated without a scheduled timeout. A timed drive event does not need to finish before another is initiated. ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ #include "GlobalHeader.h" #include "SMDrivetrain.h" /*----------------------------- Module Defines ----------------------------*/ #define FullDuty 255 //Can be made into a variable to account for battery decay #define OffDuty 20 //Max duty cycle for which the motor is off. Can be a variable. //identification of left and right motors #define DRIVEMOTOR0 0 #define DRIVEMOTOR1 1 #define DriveLeftMotor DRIVEMOTOR0 #define DriveRightMotor DRIVEMOTOR1 #define AIMING_THRESHOLD_POWER 20 //minimum aiming turning power /*---------------------------- Module Functions ---------------------------*/ static Event_t DuringWaiting( Event_t Event); static Event_t DuringMoving( Event_t Event); static Event_t DuringBeaconAligning( Event_t Event); static Event_t DuringBeaconTracking( Event_t Event); static Event_t DuringAiming( Event_t Event); static void DrivetrainInit(void); static void Drive(signed char Direction, signed char Power); static void SetDriveMotor(unsigned char Motor, signed char Power); /*---------------------------- Module Variables ---------------------------*/ //state and timer variables static DriveState_t CurrentState; static unsigned int StartTime; static unsigned int TimerLength; static unsigned char TimerExpiredFlag = 0; //tracking variables static unsigned char BeaconTrackingFlag = 0; //guard on where to transition to from aligning static unsigned char TrackingPower; static Beacon_t CurrentBeacon; static signed char TrackingAngle; //new command variables static unsigned char StateTransitionFlag = 0; static DriveState_t TransitionState; static signed char TransitionDirection; static signed char TransitionPower; static signed char TransitionAngle; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function QueryDriveTransitionFlag Parameters none Returns 1 if flag was set, 0 otherwise Description Checks to see if DrivetrainSM should make a state change ****************************************************************************/ unsigned char QueryDriveTransitionFlag(void) { unsigned char ReturnVal = StateTransitionFlag; StateTransitionFlag = 0; return ReturnVal; } /**************************************************************************** Function Stop Parameters none Returns none Description Stops motors, without creating a timed drive. ****************************************************************************/ void Drive_Stop(void) { StateTransitionFlag = 1; TransitionState = Drive_Waiting; TransitionDirection = 0; TransitionPower = 0; } /**************************************************************************** Function Drive_Timed Parameters signed char Direction, signed char Power, unsigned int Timer (in ms) Returns none Description Initiates the robot driving in specified direction at specified power for specified time ****************************************************************************/ void Drive_Timed(signed char Direction, signed char Power, unsigned int Timer) { StateTransitionFlag = 1; TransitionState = Drive_Moving; TransitionDirection = Direction; TransitionPower = Power; TimerLength = Timer; } /**************************************************************************** Function QueryDriveTimerFlag Parameters none Returns 1 if timer has expired, 0 otherwise Description Checks to see if drive timer has expired ****************************************************************************/ unsigned char QueryDriveTimerFlag(void) { unsigned char ReturnVal = 0; if (CurrentState == Drive_Moving) ReturnVal = TimerExpiredFlag; TimerExpiredFlag = 0; return ReturnVal; } /**************************************************************************** Function Drive_AlignToBeacon Parameters Beacon_t Beacon - beacon to align to, signed char Angle - relative alignment angle (turns turret to achieve offset), unsigned char Power - inital turning speed. Returns none Description Angles turret and aligns to specified beacon ****************************************************************************/ void Drive_AlignToBeacon(Beacon_t Beacon, signed char Angle, unsigned char Power) { StateTransitionFlag = 1; BeaconTrackingFlag = 0; CurrentBeacon = Beacon; TransitionAngle = Angle; TransitionState = Drive_BeaconAligning; TransitionPower = Power; } /**************************************************************************** Function QueryDriveBeaconFound Parameters none Returns 1 if robot is aligning and the desired beacon is seen straight ahead Description checks for whether beacon alignment has been achieved ****************************************************************************/ unsigned char QueryDriveBeaconFound(void) { unsigned char ReturnVal = 0; if (CurrentState == Drive_BeaconAligning) ReturnVal = (QueryBeaconDirection(CurrentBeacon) == Beacon_Straight); return ReturnVal; } /**************************************************************************** Function Drive_TrackBeacon Parameters Beacon_t Beacon - beacon to track, signed char Angle - relative alignment angle (turns turret to achieve offset), unsigned char Power - initial tracking power Returns none Description angles turret and initiates robot driving to desired beacon. angling turret allows for approaching the beacon in an arced movement. angle and power can be updated during tracking. ****************************************************************************/ void Drive_TrackBeacon(Beacon_t Beacon, signed char Angle, unsigned char Power) { StateTransitionFlag = 1; BeaconTrackingFlag = 1; CurrentBeacon = Beacon; TransitionAngle = Angle; TransitionState = Drive_BeaconAligning; TransitionPower = Power; } /**************************************************************************** Function QueryDriveBeaconLost Parameters none Returns 1 if beacon was lost during tracking, 0 otherwise Description Checks to see if beacon was lost during tracking ****************************************************************************/ unsigned char QueryDriveBeaconLost(void) { unsigned char ReturnVal = 0; if (CurrentState == Drive_BeaconTracking) switch(QueryBeaconDirection(CurrentBeacon)) { case Beacon_Left: case Beacon_Straight: case Beacon_Right: break; default: //otherwise, we don't see beacon ReturnVal = 1; break; } return ReturnVal; } /**************************************************************************** Function ReduceTrackingPower Parameters unsigned char divisor Returns none Description Reduces the current tracking power by a factor of Divisor, to a minimum of 25. ****************************************************************************/ void ReduceTrackingPower(unsigned char Divisor) { TrackingPower = TrackingPower / Divisor; TrackingPower = ((TrackingPower < 25) ? 25 : TrackingPower); } /**************************************************************************** Function ReduceTrackingAngle Parameters unsigned char divisor Returns none Description Reduces the turret angle by a factor of divisor. ****************************************************************************/ void ReduceTrackingAngle(unsigned char Divisor) { TrackingAngle = TrackingAngle / Divisor; SetTurretServo(TrackingAngle); TransitionAngle = TransitionAngle / Divisor; } /**************************************************************************** Function Drive_Aim Parameters Beacon_t Beacon - beacon to aim at, signed char Angle - offset of where to aim relative to beacon, unsigned char Power - initial turning power Returns none Description Aims relative to specified beacon by specified angle. ****************************************************************************/ void Drive_Aim(Beacon_t Beacon, signed char Angle, unsigned char Power) { StateTransitionFlag = 1; CurrentBeacon = Beacon; TransitionState = Drive_Aiming; TransitionPower = Power; TransitionAngle = Angle; } /**************************************************************************** Function QueryDriveAimed Parameters none Returns 1 if aiming accomplished, 0 otherwise Description Checks to see if robot has aimed yet ****************************************************************************/ unsigned char QueryDriveAimed(void) { if (CurrentState == Drive_Aiming) { if (TrackingPower < AIMING_THRESHOLD_POWER) return 1; } return 0; } /**************************************************************************** Function Drive Parameters signed char direction, signed char power Returns none Description Calculates desired motor speeds based on input direction and power, then sets motors accordingly. Notes //direction 100 is left in place, 0 is straight, 100 is right in place. //power 100 is forward full, -100 is backwards full. ****************************************************************************/ static void Drive(signed char Direction, signed char Power) { signed char absDirection = (signed char)((Direction >= 0) ? Direction : -Direction); signed char LeftMotorSpeed = (signed char)(((signed int)Direction*Power <= 0) ? Power : ((100 - 2*absDirection)*(signed int)Power/100)); signed char RightMotorSpeed = (signed char)(((signed int)Direction*Power >= 0) ? Power : ((100 - 2*absDirection)*(signed int)Power/100)); SetDriveMotor(DriveLeftMotor, LeftMotorSpeed); SetDriveMotor(DriveRightMotor, RightMotorSpeed); } /**************************************************************************** Function SetDriveMotor Parameters unsigned char Motor, signed char Power Returns none Description sets specified motor to desired power, changing polarity based on the sign of power and duty based on the magnitude of power. scales and offsets power based on battery health, if desired. ****************************************************************************/ static void SetDriveMotor(unsigned char Motor, signed char Power) { unsigned char Direction = (Power >= 0); //1 if forward, 0 if backwards unsigned int absPower = (unsigned int)((Power >= 0) ? Power : -Power); unsigned char ScaledPower = (unsigned char)(absPower * (FullDuty - OffDuty) / 100 + OffDuty); ScaledPower = ((Power == 0) ? 0 : ScaledPower); //if power == 0, ignore duty offset and set to 0 switch (Motor) { case DRIVEMOTOR0: switch (Direction) { case 1: //this motor forward PTU &= BIT4LO; break; case 0: //this motor backward PTU |= BIT4HI; break; } PWMDTY0 = ScaledPower; break; case DRIVEMOTOR1: switch (Direction) { case 1: //this motor forward PTU &= BIT5LO; break; case 0: //this motor backward PTU |= BIT5HI; break; } PWMDTY1 = ScaledPower; break; } } /**************************************************************************** Function DrivetrainInit Parameters none Returns none Description Initialization of PWM during startup of statemachine ****************************************************************************/ static void DrivetrainInit(void) { //PWM Init DDRU |= (BIT0HI|BIT1HI|BIT4HI|BIT5HI); //Make ports output PTU |= (BIT4HI | BIT5HI); //Initialize motors forward PWMCLK |= (_S12_PCLK0|_S12_PCLK1); //set it to use the scaled clock PWMPRCLK |= BIT0HI; // divide by 2 for 12MHz PWMSCLA = 1; // 12MHz/(1*2) = 6MHz PWMPOL |= (BIT0HI|BIT1HI); //Polarity select 1 = initally high PWMPER0 = 255; //Base period of 255 ticks, makes it 24kHz, divided by PWMSCALA PWMPER1 = 255; PWMDTY0 = 128; //initialize PWM duty cycles PWMDTY1 = 128; MODRR |= (_S12_MODRR0 | _S12_MODRR1); //map to port U PWME |= _S12_PWME0 | _S12_PWME1; //enable PWM TMRS12_Init(TMRS12_RATE_1MS); //Initialize old timer library at 1ms } /**************************************************************************** Function RunDriveSM Parameters Event_t Event Returns Event_t Event Description Runs Drivetrain State machine ****************************************************************************/ Event_t RunDriveSM( Event_t CurrentEvent ) { unsigned char MakeTransition = FALSE;/* are we making a state transition? */ DriveState_t NextState = CurrentState; switch ( CurrentState ) { case Drive_Waiting : CurrentEvent = DuringWaiting(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_Transition : //if a transition has been signaled NextState = TransitionState; //transition to indicated state MakeTransition = TRUE; break; } } break; case Drive_Moving : CurrentEvent = DuringMoving(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_Transition : //if a transition has been signaled NextState = TransitionState; //transition to indicated state MakeTransition = TRUE; break; case EV_Drive_TimerExpired : // on timeout Drive(0,0); //stop NextState = Drive_Waiting;//transition back to waiting MakeTransition = TRUE; break; } } break; case Drive_BeaconAligning : CurrentEvent = DuringBeaconAligning(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_Transition : //if a transition has been signaled NextState = TransitionState; //transition to indicated state MakeTransition = TRUE; break; case EV_Drive_BeaconFound : //if we found the beacon Drive(0,0); //stop switch (BeaconTrackingFlag) { case 1: //if we are driving to the beacon, start tracking NextState = Drive_BeaconTracking; break; default: //if we were just aligning, go back to waiting NextState = Drive_Waiting; break; } MakeTransition = TRUE; break; } } break; case Drive_BeaconTracking : CurrentEvent = DuringBeaconTracking(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_Transition : //if a transition has been signaled NextState = TransitionState; //transition to indicated state MakeTransition = TRUE; break; case EV_Drive_BeaconLost : //if we have lost the beacon Drive(0,0); //stop NextState = Drive_BeaconAligning; //go back to alignment MakeTransition = TRUE; break; } } break; case Drive_Aiming : CurrentEvent = DuringAiming(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_Transition : //if a transition has been signaled NextState = TransitionState; //transition to indicated state MakeTransition = TRUE; break; case EV_Drive_Aimed : //If we have aimed to beacon Drive(0,0); //stop NextState = Drive_Waiting; //go back to waiting MakeTransition = TRUE; break; } } break; } // If we are making a state transition if (MakeTransition == TRUE) { // Execute exit function for current state (void)RunDriveSM(EV_EXIT); CurrentState = NextState; //Modify state variable //printf("Entering Drive State %dr\n",CurrentState); // Execute entry function for new state (void)RunDriveSM(EV_ENTRY); } return(CurrentEvent); } /**************************************************************************** Function StartDriveSM Parameters Event_t Event Returns Event_t Event Description Starts drivetrain SM ****************************************************************************/ void StartDriveSM ( Event_t CurrentEvent ) { //initialize drivetrain and servos DrivetrainInit(); ServosInit(); printf("Starting Drivetrain\r\n"); CurrentState = Drive_Waiting; // call the entry function (if any) for the ENTRY_STATE (void)RunDriveSM(EV_ENTRY); } /**************************************************************************** Function QueryDriveSM Parameters none Returns DriveState_t Current State Description returns state of machine ****************************************************************************/ DriveState_t QueryDriveSM ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ /**************************************************************************** Function DuringWaiting Parameters Event_t Event Returns Event_t Event Description Stops motors on exit. ****************************************************************************/ static Event_t DuringWaiting( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { }else if ( Event == EV_EXIT) { Drive(0, 0); //always stop when leaving, in case game is ending }else // do the 'during' function for this state { } return(Event); } /**************************************************************************** Function DuringMoving Parameters Event_t Returns Event_t Description Initiates timed movement on entry, Checks if timed movement is over, stops motors on exit. ****************************************************************************/ static Event_t DuringMoving( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { //start moving and start timer Drive(TransitionDirection, TransitionPower); StartTime = TMRS12_GetTime(); TimerExpiredFlag = 0; }else if ( Event == EV_EXIT) { Drive(0, 0);//always stop when leaving, in case game is ending }else // do the 'during' function for this state { //check if timer expired. if so, set flag if(TMRS12_GetTime() - StartTime > TimerLength) TimerExpiredFlag = 1; } return(Event); } /**************************************************************************** Function DuringBeaconAligning Parameters Event_t Returns Event_t Description initiates alignment on entry, updates turning during, and stops on exit ****************************************************************************/ static Event_t DuringBeaconAligning( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { TrackingPower = TransitionPower; TrackingAngle = TransitionAngle; SetTurretServo(TrackingAngle); }else if ( Event == EV_EXIT) { Drive(0, 0);//always stop when leaving, in case game is ending }else // do the 'during' function for this state { BeaconDirection_t Direction = QueryBeaconDirection(CurrentBeacon); switch (Direction) //turn in direction of beacon { case Beacon_LostLeft: Drive(100, TrackingPower); break; case Beacon_Left: Drive(100, TrackingPower/2); break; case Beacon_Straight: Drive(0, 0); break; case Beacon_Right: Drive(-100, TrackingPower/2); break; case Beacon_LostRight: Drive(-100, TrackingPower); break; case Beacon_NeverSeen: Drive(100, TrackingPower); break; } } return(Event); } /**************************************************************************** Function DuringBeaconTracking Parameters Event Returns Event Description Updates driving based on beacon location. stops on exit. ****************************************************************************/ static Event_t DuringBeaconTracking( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { }else if ( Event == EV_EXIT) { Drive(0, 0);//always stop when leaving, in case game is ending }else // do the 'during' function for this state { BeaconDirection_t Direction = QueryBeaconDirection(CurrentBeacon); switch (Direction) //drive in direction of beacon { case Beacon_LostLeft: case Beacon_Left: Drive(25, TrackingPower); break; case Beacon_Straight: Drive(0, TrackingPower); break; case Beacon_Right: case Beacon_LostRight: Drive(-25, TrackingPower); break; case Beacon_NeverSeen: Drive(0,0); break; } } return(Event); } /**************************************************************************** Function DuringAiming Parameters Event Returns Event Description Sets Turret and initializes aiming on entry. Updates driving based on beacon location. stops on exit. ****************************************************************************/ static Event_t DuringAiming( Event_t Event) { static BeaconDirection_t LastDirection; // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { SetTurretServo(TURRET_AIM - TransitionAngle); TrackingPower = TransitionPower; LastDirection = Beacon_NeverSeen; }else if ( Event == EV_EXIT) { Drive(0, 0);//always stop when leaving, in case game is ending }else // do the 'during' function for this state { BeaconDirection_t Direction = QueryBeaconDirection(CurrentBeacon); switch (Direction) //turn so that beacon lines up with the right edge of left detector { case Beacon_LostLeft: case Beacon_Left: Drive(100, TrackingPower); break; case Beacon_Straight: if (LastDirection == Beacon_Right) TrackingPower /= 2; Drive(100, TrackingPower/2); break; case Beacon_Right: if (LastDirection == Beacon_Straight) TrackingPower /= 2; Drive(-100, TrackingPower/2); break; case Beacon_LostRight: case Beacon_NeverSeen: Drive(-100, TrackingPower); break; } LastDirection = Direction; } return(Event); }