//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);
}
public StepperMotorController(CTRE.Phoenix.CANifier CANifier, CTRE.Phoenix.CANifier.GeneralPin directionPin, Microsoft.SPOT.Hardware.Cpu.PWMChannel movePin, uint maxSpeed,
CTRE.Phoenix.CANifier.GeneralPin forwardLimitSwitchPin, CTRE.Phoenix.CANifier.GeneralPin reverseLimitSwitchPin)
{
lastDirection = Direction.STOPPED;
this.maxSpeed = maxSpeed;
this.directionPort = directionPin;
this.forwardLimitSwitch = forwardLimitSwitchPin;
this.reverseLimitSwitch = reverseLimitSwitchPin;
this.CANifier = CANifier;
movePort = new PWM(movePin, maxSpeed, maxSpeed / 2, PWM.ScaleFactor.Microseconds, false);
}