218B Project Pseudocode Joaquin Castillo -------------------------------------------------------------------------------- BeaconFSM InitBeaconFSM() assign priority initialize beacon hardware RunBeaconFSM() SWITCH CurrentState CASE BeaconDisabled IF event is BEACON_ENABLE reset IC module and interrupts change states to Beaconenabled CASE BeaconEnabled IF event is BEACON_PULSE calculate period in us determine team IF team not determined restart beacon ELSE post turn on LED post team determined to GameFSM disable beacon change states to BeaconDisabled InitBeacon() Initialize input capture module (IC2) enabel interrupts globally enable IC module EnableBeacon() reset team selection reset IC module clear IC interrupt flag enable beacon IC interrupt locally DisableBeacon() disable IC module disable beacon interrupt locally Beacon_Int_Handler() IF PulseNumber < 2 allow for two pulses to go by ELSE IF PulseNumber == 2 capture time on 3rd pusle ELSE IF PulseNumber == 3 capture time on 4th pulse ELSE post beacon event after times are captured clear interrupt flag -------------------------------------------------------------------------------- InitDriveFSM() assign priority initialize motor hardware enable interrupts globally disable motors set CurrentState to Stopped RunDriveFSM() SWITCH CurrentState CASE Stopped SWITCH EventType CASE DRIVE_FORWARD set directions start driving set current state to driving CASE DRIVE_BACKWARD set directions start driving set current state to driving CASE TURN_LEFT set directions convert angle (deg) to cm start driving set current state to driving CASE TURN_RIGHT set directions convert angle (deg) to cm start driving set current state to driving CASE DRIVING SWITCH EventType CASE ALL_STOP disable motors set current state to stopped InitDriveMotors() Initialize timebase timer (Timer2) Initialize left output compare module (OC3 on RPA3) Initialize right output compare module (OC4 on RPA4) Initialize left direction pin (RPA0) Initialize right direction pin (RPA1) Enable hardware InitDriveEncoders() Initialize timbase timer (Timer3) Initialize left stall timer (Timer 1) Initialize right stall timer (Timer 4) Initialize left input capture module (IC1) Initialize right inpurt caputre module (IC4) turn on hardware InitDriveControlTimer() Initialize timer (Timer 5) Enable hardware StartDriving() set target to EventParam set current positions to zero initialize error terms enable left encoder ISR enable right encoder ISR enable left stall timer ISR enable right stall timer ISR enable control law ISR StopDriving() disable control law ISR disable left stall timer ISR disable right stall timer ISR disable left encoder ISR disable right encoder ISR reset directions set left duty cycle to zero set right duty cycle to zero SetLeftDir set direction pin to param value set module variable to param value SetRightDir oppose of param value used to compensate for mirrored motor behavior set direction pin to !param value set module variable to !param value _Drive_Encoder_L() read IC into NewTime subtract LastTime from NewTime to get pulse period set LastTime to NewTime increment current position convert PulsePeriod to RPM and save to EncoderRPM reset stall timer clear interrupt flag _Stall_L() set encoder rpm to 0 clear interrupt flag _Drive_Encoder_R() read IC into NewTime subtract LastTime from NewTime to get pulse period set LastTime to NewTime increment current position convert PulsePeriod to RPM and save to EncoderRPM reset stall timer clear interrupt flag _Stall_R() set encoder rpm to 0 clear interrupt flag _Drive_Control_Law_ISR() define gains update position errors calculate error sums IF below error treshold post all stop event to DriveFSM post position reached event to GameFSM update speed errors calculate speed error sums calculate control duty cycles anti-windup for saturation update motor duty cycles clear interrupt flag -------------------------------------------------------------------------------- GameFSM InitGameFSM() assign priority initialize button interrupt initialize game timer enable button interrupt set state to standby RunGameFSM() IF EventType == GAME_TIMEOUT SWITCH CurrentState CASE Standby IF EventType == BUTTON_PRESSED start game timer post raise flag to flag service post enable to beacon service post turn left to drive fsm post turn on to LED service set state to finding beacon CASE FindingBeacon IF EventType == TEAM_DETERMINED turn right to compensate for beacon sensor offset state to readjusting CASE Readjusting IF EventType == POSITION_REACHED driving forward to shooting range state to driving to shoot CASE DrivingToShoot IF EventType == POSITION_REACHED post enable to beacon service post turn left 360 deg to drive fsm set state to aiming CASE Aiming IF EventType == TEAM_DETERMINED OR EventType == POSITION_REACHED post spin up to shooter fsm via spi service post shoot em! to shooter fsm via spi service set state to shooting CASE Shooting IF EventType == DONE_SHOOTING post drive backwards to driving service start timeout timer for driving to reload incase motors stall set state to driving to reload CASE DrivingToReload IF EventType == POSITION_REACHED OR EventType == ES_TIMEOUT post all stop to drive fsm enable button interrupt eat more tacos set state to reloading CASE Reloading IF EventType == BUTTON_PRESSED post drive forward to drive fsm set state to driving to shoot InitButton() Initialize external interrupt (INT3) clear interrupt priority enable interrupts globally EnableButton() clear interrupt flag enable local interrupt InitGameTimer() Initialize game timer (Timer3) enable global interrupts enable timer EnableGameTimer() reset timer overflow clear interrupt flag enable interrupt _Button_ISR() disable button interrupt post button pressed event clear interrupt flag _Game_Timer_ISR() IF less than 138 seconds increment overflow ELSE time's up reset timer overflow disable interrupt clear button flag post game timeout event clear interrupt flag -------------------------------------------------------------------------------- LED_Service InitLED() assign priority initialize pins for LED strip colors turn off LED strip off to start RunLED() SWITCH EventType CASE LED_ON IF team is red set red LED clear blue LED ELSE IF team is blue clear red LED set blue LED ELSE IF team not selected set red LED to make purple set blue LED to make purple CASE LED_OFF clear red LED clear blue LED -------------------------------------------------------------------------------- SPI_LeaderService InitSPI_LeaderService() assign priority initialize spi module send initial querry initialize query timer RunSPI_LeaderService() SWITCH EventType CASE ES_TIMEOUT read in new command decode command SWITCH Command CASE SHOOTING_DONE2Game post command event send command from command buffer reset command buffer to query CASE SEND_SPI_COMMAND IF command buffer "empty" i.e. no set command buffer to event param ELSE repost event InitSPI_L() disable SPI module configure SPI module in leader mode enable SPI module wait for module to finish initializing -------------------------------------------------------------------------------- ShooterFSM InitShooterFSM() assign priority initialize motor hardware initialize servo hardware enable interrupts globally disable flywheel set gating servo to initial position set state to flywheel off RunShooterFSM() IF EventType == ALL_STOP stop flywheel set state to flywheel off SWITCH CurrentState CASE FlywheelOff IF EventType == SPIN_UP turn on flywheel start spin up timer set state to Spinning Up CASE SpinningUp IF EventType == ES_TIMEOUT set state to Flywheel Ready ELSE IF EventType == SHOOT_EM post this event again CASE FlywheelReady IF EventType == SHOOT_EM turn gating servo initialize timeout timer set state to Shooting First CASE ShootingFirst IF EventType == ES_TIMEOUT turn gating servo initialize timeout timer set state to Shooting Second CASE ShootingSecond IF EventType == ES_TIMEOUT turn gating servo initialize timeout timer set state to Shooting Third CASE ShootingThird IF EventType == ES_TIMEOUT post SHOOTING DONE to Game HSM turn off flywheel reset gating servo set state to FlywheelOff InitShooterMotor() Initialize timebase timer (Timer2), also used by servo, configed 50Hz Initialize output compare module (OC4 on RPA2) Enable hardware InitShooterServo() Initialize output compare module (OC5 on RPA4) Enable hardware StartFlywheel() set duty cycle to desired value StopFlywheel() set duty cycle to zero SetGatePosition() set servo to position param -------------------------------------------------------------------------------- TacoFlagService InitTacoFlag() assign priority initialize servo hardware set servo to initial position RunTacoFlag() SWITCH EventType CASE RAISE_FLAG turn taco flag servo up CASE LOWER_FLAG turn taco flag servo down InitTacoServo() Initialize output compare module (OC3 on RB9) Enable hardware SetTacoPosition() set servo to position param -------------------------------------------------------------------------------- SPI_FollowerService InitSPI_FollowerService() assign priority initialize spi module RunSPI_FollowerService() SWITCH EventType CASE SEND_SPI_COMMAND IF query in command buffer set command buffer to event param ELSE another command is already in the buffer repost event CASE RECEIVE_SPI_COMMAND decode command SWITCH EventParameter CASE SPIN_UP2Shooter post command event CASE SHOOT_EM2Shooter post command event CASE RAISE_FLAG2TacoFlag post command event CASE LOWER_FLAG2TacoFlag post command event InitSPI_F() disable module configure spi module in follower mode enable module wait for module to finish initializing _SPI_RX_ISR() read in from SPI buffer IF command not a query post received command with command as param set SPI buffer to command buffer reset command buffer to querry clear interrupt flag