//Setup public Excavation() { excavationState = ExcavationState.INIT; //Actuators leftActuator = TalonFactory.CreateLinearActuator(CAN_IDs.EXCAVATION.LEFT_ACTUATOR); rightActuator = TalonFactory.CreateLinearActuator(CAN_IDs.EXCAVATION.RIGHT_ACTUATOR); //Init Canifier for controlling stepper motors CANifier = new CTRE.Phoenix.CANifier((ushort)CAN_IDs.ACCESSORIES.CANIFIER); //Initialize the forward and reverse limit switches for extending/retracting the auger forwardLimitSwitch = new LimitSwitch(CANifier, CTRE.Phoenix.CANifier.GeneralPin.SPI_CLK_PWM0P); reverseLimitSwitch = new LimitSwitch(CANifier, CTRE.Phoenix.CANifier.GeneralPin.SPI_MOSI_PWM1P); //Stepper Motors (controlled from one motor controller object) augerExtender = new StepperMotorController(CANifier, CTRE.Phoenix.CANifier.GeneralPin.SPI_CS, CTRE.HERO.IO.Port3.PWM_Pin9, STEPPER_MAX_SPEED, CTRE.Phoenix.CANifier.GeneralPin.SPI_CLK_PWM0P, CTRE.Phoenix.CANifier.GeneralPin.SPI_MOSI_PWM1P); augerExtender.Stop(); augerRotator = TalonFactory.CreateAugerRotator(CAN_IDs.EXCAVATION.AUGER_ROTATOR); //Light sensor for detecting full excavation tube lightSensor = new LightSensor(CTRE.HERO.IO.Port8.Analog_Pin5); }
private void UpdateStateMachine(ref Controller controller) { switch (excavationState) { //Initial State case ExcavationState.INIT: if (controller.BUTTONS.Y) { excavationState = ExcavationState.RAISING; } if (controller.BUTTONS.A && reverseLimitSwitchCntr > 3) { excavationState = ExcavationState.STOWING; } StopAll(); break; //Static States case ExcavationState.STOWED: StopAll(); if (controller.BUTTONS.Y) { excavationState = ExcavationState.RAISING; } break; case ExcavationState.RAISED: leftActuator.Set(ControlMode.PercentOutput, 0); rightActuator.Set(ControlMode.PercentOutput, 0); if (controller.BUTTONS.A && reverseLimitSwitchCntr > 3) { excavationState = ExcavationState.STOWING; } Debug.Print(reverseLimitSwitch.IsPressed().ToString()); //Control auger extension stepper motors if (controller.BUTTONS.POV_UP) { augerExtender.SetSpeed(-.5f); } else if (controller.BUTTONS.POV_DOWN) { augerExtender.SetSpeed(.2f); } else { augerExtender.Stop(); } if (controller.BUTTONS.X) { augerRotator.Set(ControlMode.PercentOutput, 0.3); } else if (controller.BUTTONS.B) { augerRotator.Set(ControlMode.PercentOutput, -0.3); } else { augerRotator.Set(ControlMode.PercentOutput, 0); } break; //Dynamic States case ExcavationState.RAISING: if (controller.BUTTONS.A) { excavationState = ExcavationState.STOWING; } if (GoToActuatorPosition(ACTUATOR_RAISED_POSITION)) { excavationState = ExcavationState.RAISED; } augerExtender.Stop(); break; case ExcavationState.STOWING: if (controller.BUTTONS.Y) { excavationState = ExcavationState.RAISING; } if (GoToActuatorPosition(ACTUATOR_STOWED_POSITION)) { excavationState = ExcavationState.STOWED; } augerExtender.Stop(); break; case ExcavationState.EXCAVATING: break; case ExcavationState.DUMPING: break; default: Debug.Print("ERROR: Invalid Excavation State Accessed!"); break; } }