private void TankDrive(ref XboxController controller)
{
double LEFT_SPEED = controller.AXES.LEFT_Y;
double RIGHT_SPEED = controller.AXES.RIGHT_Y;
SetSpeeds(LEFT_SPEED, RIGHT_SPEED);
}
private void RunActuators(ref XboxController controller)
{
//Update Encoders
Utils.Print("R, L CURRENT");
Utils.Print(OUTPUT);
Utils.Print(RightActuator.GetSelectedSensorPosition());
Utils.Print("");
//Run Motors
if (controller.POV == controller.POV_UP)
{
OUTPUT = 300;
}
else if (controller.POV == controller.POV_DOWN)
{
OUTPUT = 800;
}
//if ((OUTPUT < RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() > 1)
// || (OUTPUT > RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() < 1))
//{
// LeftActuator.Set(ControlMode.PercentOutput, 0);
// RightActuator.Set(ControlMode.PercentOutput, 0);
//}
//else
//{
LeftActuator.Set(ControlMode.MotionMagic, OUTPUT);
RightActuator.Set(ControlMode.MotionMagic, OUTPUT);
//}
}
private void RunBelts(ref XboxController controller)
{
double SPEED = 1;
if (controller.BUTTONS.RB)
{
SPEED = -.7;
}
if (controller.BUTTONS.A)
{
ExcavationBelt.Set(ControlMode.PercentOutput, SPEED);
}
else
{
ExcavationBelt.Set(ControlMode.PercentOutput, 0);
}
if (controller.BUTTONS.Y)
{
CollectionBelt.Set(ControlMode.PercentOutput, SPEED);
}
else
{
CollectionBelt.Set(ControlMode.PercentOutput, 0);
}
}
public void Drive(ref XboxController controller, bool enabled)
{
if (enabled && MODE == TANK)
{
TankDrive(ref controller);
}
else
{
Stop();
}
}
public void RunLift(ref XboxController controller, bool enabled)
{
if (enabled)
{
RunActuators(ref controller);
RunBelts(ref controller);
RunDigger(ref controller);
}
else
{
Stop();
}
}
private void RunDigger(ref XboxController controller)
{
if (controller.BUTTONS.LT)
{
Digger.Set(ControlMode.PercentOutput, 1);
}
else if (controller.BUTTONS.RT)
{
Digger.Set(ControlMode.PercentOutput, -1);
}
else
{
Digger.Set(ControlMode.PercentOutput, 0);
}
}