/*---------------------------------------------------------------------------- ** ** Weapons.c ** ** Controls weapons. ** **--------------------------------------------------------------------------*/ #include "Onboard.h" #include "Hardware.h" #include "Commands.h" #include "Weapons.h" #include "Sensors.h" // These are empirical timings in units of 10ms. #define FLIPPER_EXTEND_TIME 100 // 1 second #define FLIPPER_RETRACT_TIME 100 // 1 second #define PNEUMATICS_TIME 50 // 0.5 seconds typedef enum { SPEAR_IDLE, SPEAR_FORWARD_DRIVE, SPEAR_BACKWARD_DRIVE } TSpearState; // Enum for hammer state machine (in addition to standard FORWARDS & REVERSE) enum { PARKING=2, PARKED, PLUGGING_FORWARDS, // These two states enable the motor rto be slowed PLUGGING_REVERSE // down just as it enters the index position. }; // Enum for flipper control state machines enum { FLIPPER_PARKED=2, EXTENDING_FORWARDS, EXTENDING_BACKWARDS, EXTENDED_FORWARDS, EXTENDED_BACKWARDS, RETRACTING, FLIPPING }; // In addition to the general forwards/backwards enum #define PARK 2 #define DRIVE 2 // Forwards or backwards // Globals within this module static char HammerState[2]; static char HammerRequest[2]; static char HammerMotor[2]; static char HammerDirection[2]; static char HammerSolenoid[2]; static char HammerIndex[2]; static char HammerDriveLevel = 255; // How hard hammer motors are driven (PWM / 256) static char FlipperState = FLIPPER_PARKED; static int FlipperTimer, PneumaticsTimer; static char LastFlipperPosition = DOWN; static char LastFlipperExtension = FORWARDS; static TSpearState SpearStateVar = SPEAR_IDLE; /*---------------------------------------------------------------------------- // // InitialiseWeapons // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void InitialiseWeapons(void) { // Set up spear interrupts. ENABLE_SPEAR_INTS; } /*---------------------------------------------------------------------------- // // LegAssist // // The leg/hammers are to assist in motion. They must drive in the correct // direction to assist the requested direction of motion. This depends on // which way up the robot is as well. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void LegAssist(void) { if ( (Speed < 0) ^ (WhichWayUp == RIGHT_WAY_UP) ) HammerRequest[FRONT] = HammerRequest[REAR] = FORWARDS; else HammerRequest[FRONT] = HammerRequest[REAR] = REVERSE; } /*---------------------------------------------------------------------------- // // FrontHammerStateMachine // // Control the hammer I/O lines by use of a state machine. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void HammerStateMachine(char Hammer) { // // Park both hammers in emergency // if (SpecialsRequest[EMERGENCY]) { if (HammerState[0] != PARKED) HammerState[0] = PARKING; if (HammerState[1] != PARKED) HammerState[1] = PARKING; } // // Refresh index variables. // HammerIndex[FRONT] = FRONT_HAMMER_INDEX_POSITION; HammerIndex[REAR] = REAR_HAMMER_INDEX_POSITION; // // Run state machine // switch (HammerState[Hammer]) { // ------- PARKED --------- case PARKED: if (HammerRequest[Hammer] == FORWARDS || HammerRequest[Hammer] == BACKWARDS) { HammerSolenoid[Hammer] = OFF; // Release park solenoid HammerDirection[Hammer] = HammerRequest[Hammer]; HammerMotor[Hammer] = HammerDriveLevel; HammerState[Hammer] = HammerRequest[Hammer]; } break; // ------- PARKING --------- case PARKING: if (HammerRequest[Hammer] == FORWARDS || HammerRequest[Hammer] == BACKWARDS) { // Command to drive has arrived while waiting for park index. HammerSolenoid[Hammer] = OFF; // Release park solenoid HammerDirection[Hammer] = HammerRequest[Hammer]; HammerMotor[Hammer] = HammerDriveLevel; HammerState[Hammer] = HammerRequest[Hammer]; } else if (FRONT_HAMMER_INDEX_POSITION) { // As soon as index position is detected, turn off the motor. // NOTE: It may be better to reverse the motor direction for a few // tens of milliseconds to slow the hammer down to prevent damage // the the index solenoid. This will require more states. HammerMotor[Hammer] = OFF; HammerState[Hammer] = PARKED; } break; // ------- FORWARDS --------- case FORWARDS: if (HammerRequest[Hammer] == BACKWARDS) { HammerDirection[Hammer] = BACKWARDS; HammerState[Hammer] = BACKWARDS; } else if (HammerRequest[Hammer] == PARK) { // Activate the park solenoid which will slide along the // wheel unitl it clicks into the index hole. HammerSolenoid[Hammer] = ON; HammerState[Hammer] = PARKING; } break; // ------- BACKWARDS --------- case BACKWARDS: if (HammerRequest[Hammer] == FORWARDS) { HammerDirection[Hammer] = FORWARDS; HammerState[Hammer] = FORWARDS; } else if (HammerRequest[Hammer] == PARK) { // Activate the park solenoid which will slide along the // wheel until it clicks into the index hole. HammerSolenoid[Hammer] = ON; HammerState[Hammer] = PARKING; } break; } // // Set appropriate I/O lines based on variable states // FRONT_HAMMER_PWM = HammerMotor[FRONT]; REAR_HAMMER_PWM = HammerMotor[REAR]; FRONT_HAMMER_HOME_SOLENOID = HammerSolenoid[FRONT]; REAR_HAMMER_HOME_SOLENOID = HammerSolenoid[REAR]; FRONT_HAMMER_MOTOR_DIRECTION = HammerDirection[FRONT]; REAR_HAMMER_MOTOR_DIRECTION = HammerDirection[REAR]; } /*---------------------------------------------------------------------------- // // ProcessHammers // // Deals with hammer requests. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ProcessHammers(void) { static char FrontHammerState = PARKED; static char RearHammerState = PARKED; // // If leg assist for motion mode is active, work out which direction // the hammers must go in to help the requested motion. // if (SpecialsRequest[LEG_ASSIST]) LegAssist(); // // No emergency, use standard state machines. // HammerStateMachine(FRONT); HammerStateMachine(REAR); } /*---------------------------------------------------------------------------- // // ActivateSolenoid // // Set solenoid valves to move flipper. Air circuit is as follows: // // | // X--SV0 SV1 // Piston 1 | | // ----HHHHHHHH================X======| // Down | // | Reservoir // |===HHHHHHHHH // | // Piston 2 | // ----HHHHHHHH================X======| // Up | | // X--SV2 SV3 // | // // Parameters: // Setting: TOGGLE, PNEUMATICS_OFF, or PNEUMATICS_PURGE // // Modification Record: // 26-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ActivateSolenoid(char Setting) { if (Setting == DOWN) { FLIPPER_SOLENOID_VALVE_0 = CLOSED; FLIPPER_SOLENOID_VALVE_1 = OPEN; FLIPPER_SOLENOID_VALVE_2 = OPEN; FLIPPER_SOLENOID_VALVE_3 = CLOSED; } else if (Setting == UP) { FLIPPER_SOLENOID_VALVE_0 = OPEN; FLIPPER_SOLENOID_VALVE_1 = CLOSED; FLIPPER_SOLENOID_VALVE_2 = CLOSED; FLIPPER_SOLENOID_VALVE_3 = OPEN; } else if (Setting == PNEUMATICS_OFF) { FLIPPER_SOLENOID_VALVE_0 = CLOSED; FLIPPER_SOLENOID_VALVE_1 = CLOSED; FLIPPER_SOLENOID_VALVE_2 = CLOSED; FLIPPER_SOLENOID_VALVE_3 = CLOSED; } else if (Setting == PNEUMATICS_PURGE) { FLIPPER_SOLENOID_VALVE_0 = OPEN; FLIPPER_SOLENOID_VALVE_1 = OPEN; FLIPPER_SOLENOID_VALVE_2 = OPEN; FLIPPER_SOLENOID_VALVE_3 = OPEN; } } /*---------------------------------------------------------------------------- // // TogglePneumatics // // Flip the flipper! // // Modification Record: // 26-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void TogglePneumatics(void) { if (LastFlipperPosition == UP) ActivateSolenoid(DOWN); else ActivateSolenoid(UP); LastFlipperPosition = 1 - LastFlipperPosition; } /*---------------------------------------------------------------------------- // // ComplexFlipperStateMachine // // State machine for Extend->flip->Retract motions. // // Modification Record: // 26-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ComplexFlipperStateMachine(void) { switch(FlipperState) { //-------- FLIPPER_PARKED ---------- case FLIPPER_PARKED: { if (FlipperTimer == 0) { if (WeaponsRequest[FLIPPER_FORWARDS]) { // Extend flipper forwards... FlipperTimer = FLIPPER_EXTEND_TIME; FLIPPER_RETRACT_MOTOR_POS = ON; FLIPPER_RETRACT_MOTOR_NEG = OFF; FlipperState = EXTENDING_FORWARDS; } else // Must be FLIPPER_BACKWARDS { // Extend flipper backwards... FlipperTimer = FLIPPER_EXTEND_TIME; FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = ON; FlipperState = EXTENDING_BACKWARDS; } // Switch it so it starts off in the down position, ready to flip. if (WhichWayUp == RIGHT_WAY_UP) ActivateSolenoid(DOWN); else ActivateSolenoid(UP); PneumaticsTimer = PNEUMATICS_TIME; } } break; //-------- EXTENDING_BACKWARDS ---------- case EXTENDING_BACKWARDS: { if (FlipperTimer == 0) { FLIPPER_RETRACT_MOTOR_NEG = OFF; TogglePneumatics(); LastFlipperExtension = BACKWARDS; FlipperState = FLIPPING; } } break; //-------- EXTENDING_FORWARDS ---------- case EXTENDING_FORWARDS: { if (FlipperTimer == 0) { FLIPPER_RETRACT_MOTOR_POS = OFF; TogglePneumatics(); LastFlipperExtension = FORWARDS; FlipperState = FLIPPING; } } break; //-------- FLIPPING ---------- case FLIPPING: { if (PneumaticsTimer == 0) { if (LastFlipperExtension == FORWARDS) { FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = ON; } else { FLIPPER_RETRACT_MOTOR_POS = ON; FLIPPER_RETRACT_MOTOR_NEG = OFF; } FlipperTimer = FLIPPER_RETRACT_TIME; FlipperState = RETRACTING; } } break; //-------- RETRACTING ---------- case RETRACTING: { if (FLIPPER_INDEX_POSITION) { FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = OFF; FlipperState = FLIPPER_PARKED; } } break; } } /*---------------------------------------------------------------------------- // // SimpleFlipperStateMachine // // State machine for simple flipper commands (extend and retract) // // Modification Record: // 26-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void SimpleFlipperStateMachine(void) { // // General state transitions from any state. // if (FlipperTimer == 0) { if (WeaponsRequest[EXTEND_FLIPPER_FORWARDS]) { FlipperTimer = FLIPPER_EXTEND_TIME; FLIPPER_RETRACT_MOTOR_POS = ON; FLIPPER_RETRACT_MOTOR_NEG = OFF; FlipperState = EXTENDING_FORWARDS; } if (WeaponsRequest[EXTEND_FLIPPER_BACKWARDS]) { FlipperTimer = FLIPPER_EXTEND_TIME; FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = ON; FlipperState = EXTENDING_BACKWARDS; } if (WeaponsRequest[RETRACT_FLIPPER]) { FlipperTimer = FLIPPER_RETRACT_TIME; if (FlipperState == EXTENDING_FORWARDS || FlipperState == EXTENDED_FORWARDS) { FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = ON; } else { FLIPPER_RETRACT_MOTOR_POS = ON; FLIPPER_RETRACT_MOTOR_NEG = OFF; } FlipperState = RETRACTING; } } // // Explicit state transitions // switch (FlipperState) { case EXTENDING_FORWARDS: { if (FlipperTimer == 0) { FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = OFF; FlipperState = EXTENDED_FORWARDS; } } break; case EXTENDING_BACKWARDS: { if (FlipperTimer == 0) { FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = OFF; FlipperState = EXTENDED_BACKWARDS; } } break; case RETRACTING: { if (FLIPPER_INDEX_POSITION) { FLIPPER_RETRACT_MOTOR_POS = OFF; FLIPPER_RETRACT_MOTOR_NEG = OFF; FlipperState = FLIPPER_PARKED; } } break; } } /*---------------------------------------------------------------------------- // // ProcessFlipper // // Deals with flipper requests. There are simple flipper requests like extend // retract, and flip. Then there are more complex ones such as flip forwards // and flip backwards, which will require a state machine to control. // // Modification Record: // 26-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ProcessFlipper(void) { // // Update state machine timers. // if (FlipperTimer > 0) FlipperTimer--; if (PneumaticsTimer > 0) PneumaticsTimer--; // // Deal with pneumatics commands // if (WeaponsRequest[OPERATE_FLIPPER]) { PneumaticsTimer = PNEUMATICS_TIME; TogglePneumatics(); } if (PneumaticsTimer == 0) ActivateSolenoid(PNEUMATICS_OFF); // // Flipper state machines // if (WeaponsRequest[FLIPPER_FORWARDS] || WeaponsRequest[FLIPPER_BACKWARDS]) ComplexFlipperStateMachine(); else SimpleFlipperStateMachine(); } /*---------------------------------------------------------------------------- // // SetSpearOutputs // // Given the state variable, set the spear outputs accordingly. // // Modification Record: // 10-Oct-00 Paul Hills First version -----------------------------------------------------------------------------*/ void SetSpearOutputs(void) { switch (SpearStateVar) { case SPEAR_IDLE: { SPEAR_DRIVE_FORWARDS = 0; SPEAR_DRIVE_BACKWARDS = 0; break; } case SPEAR_FORWARD_DRIVE: { SPEAR_DRIVE_FORWARDS = 1; SPEAR_DRIVE_BACKWARDS = 0; break; } case SPEAR_BACKWARD_DRIVE: { SPEAR_DRIVE_FORWARDS = 0; SPEAR_DRIVE_BACKWARDS = 1; break; } } } /*---------------------------------------------------------------------------- // // SpearForwardIsr // // Spear has appeared in the forward position. If in MTR mode, update the // state machine. // // Modification Record: // 10-Oct-00 Paul Hills First version -----------------------------------------------------------------------------*/ interrupt [IRQ_6] void SpearForwardIsr(void) { if (WeaponsRequest[SPEAR_MULTITHROW]) { SpearStateVar = SPEAR_BACKWARD_DRIVE; SetSpearOutputs(); } } /*---------------------------------------------------------------------------- // // SpearBackIsr // // Spear has appeared in the back position. If in MTR mode, update the // state machine. // // Modification Record: // 10-Oct-00 Paul Hills First version -----------------------------------------------------------------------------*/ interrupt [IRQ_7] void SpearBackIsr(void) { if (WeaponsRequest[SPEAR_MULTITHROW]) { SpearStateVar = SPEAR_FORWARD_DRIVE; SetSpearOutputs(); } } /*---------------------------------------------------------------------------- // // ProcessSpear // // Deals with spear requests. There are three types of spear request: fire // forwards, fire backwards, and multithrow, where the spear is fired back and // forth bouncing off its endsprings. The spear functionality is controlled by // the following state machine: // _____________________________________ // / MTR.Bi (ISR) \ // V | // /------\ FR /------\ BR /------\ // | |<----------| |---------->| | // | fore | | idle | | back | // | |---------->| |<----------| | // \------/ /MTR.Fi \------/ /MTR.Bi \------/ // ^ | // \________________MTR.Fi__(ISR)________/ // // Key: // MTR = Multithrow request // FR = Forward request // BR = Backward request // Fi = Forward index // Bi = backward index // // Note that the transitions from fore to back states when in multithrow // mode are performed by the ISRs for IRQ6 and IRQ7, not by the function // below, since a visit every 10ms may not be fast enough. // // Modification Record: // 10-Oct-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ProcessSpear(void) { // // This is the state machine to decide the next state. Note that // multithrow index detection events are controlled by the ISRs above. // This function just controls the standard forwards and backwards // commands. // switch (SpearStateVar) { case SPEAR_IDLE: { if (WeaponsRequest[SPEAR_FORWARDS] || WeaponsRequest[SPEAR_MULTITHROW]) SpearStateVar = SPEAR_FORWARD_DRIVE; else if (WeaponsRequest[SPEAR_BACKWARDS]) SpearStateVar = SPEAR_BACKWARD_DRIVE; break; } case SPEAR_FORWARD_DRIVE: { if (SPEAR_FORWARD_INDEX) SpearStateVar = SPEAR_IDLE; break; } case SPEAR_BACKWARD_DRIVE: { if (SPEAR_BACKWARD_INDEX) SpearStateVar = SPEAR_IDLE; break; } } } /*---------------------------------------------------------------------------- // // ProcessWeapons // // Checks the bits in the WeaponsRequest bitfield (extracted from received // radio data) to work out what actions are required. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ProcessWeapons(void) { ProcessSpear(); ProcessHammers(); ProcessFlipper(); }