//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);
}
//Initializes the 4 talons, 1 for each wheel
//Assigns a current limit of 10 A for a continous time of 150 ms
public DriveBase(short mode = TANK)
{
MODE = mode;
FrontLeft = TalonFactory.CreateDriveBaseTalon(CAN_IDs.DRIVE_BASE.FRONT_LEFT_WHEEL);
FrontRight = TalonFactory.CreateDriveBaseTalon(CAN_IDs.DRIVE_BASE.FRONT_RIGHT_WHEEL, true);
BackLeft = TalonFactory.CreateDriveBaseTalon(CAN_IDs.DRIVE_BASE.BACK_LEFT_WHEEL);
BackRight = TalonFactory.CreateDriveBaseTalon(CAN_IDs.DRIVE_BASE.BACK_RIGHT_WHEEL, true);
}