//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;
}
}
