Drivetrain and Servos Pseudocode
Drivetrain State Machine Function: Drive_Stop Mark transition to waiting state Set transition power and direction to 0 Function: Drive_Timed Parameters: signed char Direction, signed char Power, unsigned int Timer Mark transition to moving state Set transition direction, power, and timer as inputted Function: Drive_AlignToBeacon Parameters: Beacon_t Beacon, signed char Angle, unsigned char Power Mark transition to BeaconAligning state Clear beacon tracking flag Set transition direction, angle, and power as inputted Function: Drive_TrackBeacon Parameters: Beacon_t Beacon, signed char Angle, unsigned char Power Mark transition to BeaconAligning state Set beacon tracking flag Set transition direction, angle, and power as inputted Function: ReduceTrackingPower / Angle Parameters unsigned char Divisor Reduce current tracking power / angle by inputted divisor Function: Drive_Aim Parameters: Beacon_t Beacon, signed char Angle, unsigned char Power Mark transition to Aiming state Set transition power and angle as inputted Function: Drive Parameters: signed chars Direction, Power Calculate desired motor speeds and directions based on direction and power Set drive motors based on these speeds and directions Function: SetDriveMotor Parameters: unsigned char Motor, signed char Power Scale and shift Power to account for battery health If power is positive, Set Motor direction to 1 If power is negative Set Motor direction to 0 Set Motor Duty based on magnitude of Power Function: DrivetrainInit Initialize PWM for both drive motors Initialize direction outputs for both rive motors Function: StartDriveSM Initialize Drivetrain Initialize Servos module Function: RunDriveSM State Waiting: Run during function for Waiting Event: Drive transition Transition to next state State Moving: Run during function for moving Event: Drive transition Transition to next state Event: Drive timer expired Stop motors Transition to Waiting state State BeaconAligning: Event: Drive transition Transition to next state Event: Beacon found while aligning Stop motors If tracking flag is set Transition to Tracking state if tracking flag not set Transition to Waiting state State BeaconTracking: Event: Drive transition Transition to next state Event: Beacon lost while tracking Stop motors Transition to Aligning state State Aiming: Event: Drive transition Transition to next state Event: Successfully aimed Stop motors Transition to waiting state Function: DuringWaiting Exit: Stop motors Function: DuringMoving Entry: Drive based on direction and power from transition Exit: Stop motors Else: If drive time has passed, set timer expired flag Function: DuringBeaconAligning Entry: Set turret servo based on transition angle Exit: stop motors Else: If the beacon we are aligning to is … LostLeft Turn left in place full power … Left Turn left in place half power … Straight ahead Stop … Right Turn right in place half power … LostRight Turn right in place full power … NeverSeen Turn left in place full power Function: DuringBeaconTracking Entry: Set turret servo based on transition angle Exit: stop motors Else: If the beacon we are tracking is … LostLeft or Left Drive forward and to the left … Straight ahead Drive forward … Right or LostRight Drive forward and to the right … NeverSeen Stop Function: DuringAiming Entry: Set turret servo based on transition angle Exit: stop motors Else: If the beacon we are tracking is … LostLeft or Left Turn left in place full power … Straight ahead Turn left in place half power … Right Turn right in place half power … LostRight or NeverSeen Turn right in place full powerServos Module Function: ServosInit Initialize output compare to create falling edge for each servo Initialize output compare to create rising edge for all three servos (simultaneous) Function: SetTurretServo Parameter: signed char Angle Calculate what the on time should be based on inputted angle Set turret servo's on time to this value Function: SetShooterServo / SetGateServo Parameter: unsigned int OnTime Set shooter/gate servo's on time to this value Interrupt: Turret/Shooter/Gate Interrupt On output compare (automatically sets relevant signal low) Set next trigger time to at least one period in the future Clear flag Interrupt: ServoPeriodInterrupt On output compare (automatically sets all three servo signals high) Set next trigger times for individual interrupts to be the relevant OnTime from now Set this interrupt's next trigger time to be one period from now Clear flag