/// <summary>
/// Drive the specified rotations.
/// </summary>
/// <param name="leftRotations">Linear distance for the left motor to travel.</param>
/// <param name="rightRotations">Linear distance for the right motor to travel.</param>
public static void DriveRotations(double rightRotations)
{
SmartDashboard.PutNumber("Right Ticks", rightRotations);
Right1.SetEncoderPostition(0); // Reset the encoder position to 0.
Left1.SetEncoderPostition(0); // Reset the eoncoder position to 0.
RobotMap.Right1.Set(rightRotations);
RobotMap.Left1.Set(rightRotations);
}
public static void DriveFast(double desiredDistance)
{
AutoFunctions.ConfigureTalon(Right1, ConfigureType.Position, new EncoderParameters
{
AllowedError = 0,
Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative,
NominalVoltage = 0.0f,
PeakVoltage = 12f * .7f,
PIDFValues = new PIDF
{
kP = 0.151,
kI = 0,
kD = 0,
kF = 0
},
ReverseSensor = false
});
AutoFunctions.ConfigureTalon(Left1, ConfigureType.Position, new EncoderParameters
{
AllowedError = 0,
Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative,
NominalVoltage = 0.0f,
PeakVoltage = 12f * .9f,
PIDFValues = new PIDF
{
kP = 0.151,
kI = 0,
kD = 0,
kF = 0
},
ReverseSensor = true
});
Right1.SetEncoderPostition(0); // Reset the encoder position to 0.
Left1.SetEncoderPostition(0); // Reset the eoncoder position to 0.
// Converts meters to inches.
double desiredInches = 39.3701 * desiredDistance;
// Converts inches to rotations.
double rotations = desiredInches / 3.29; // 3.29 inches per rotation.
// Gets the displacement of the motors to set.
double left = /*(RobotMap.Left1 .GetEncoderPosition() / 4096 * 39.3701 / 3.29) + */ rotations;
double right = /*(RobotMap.Right1.GetEncoderPosition() / 4096 * 39.3701 / 3.29) + */ rotations;
// Drive with the specified distance.
DriveRotations(left);
SmartDashboard.PutNumber("Encoder Distance", rotations);
}
/// <summary>
/// Initializes the AutoHandler.
/// </summary>
protected override void Init()
{
// Instantiates AutoHandler.
AutoHandler = new Handler();
//Sets the CAN Talons to rely off of the encoders for movement.
AutoFunctions.ConfigureTalon(Right1, ConfigureType.Position, new EncoderParameters
{
AllowedError = 0,
Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative,
NominalVoltage = 0.0f,
PeakVoltage = 12.0f,
PIDFValues = new PIDF
{
kP = 0.151,
kI = 0,
kD = 0,
kF = 0
},
ReverseSensor = false
});
AutoFunctions.ConfigureTalon(Left1, ConfigureType.Position, new EncoderParameters
{
AllowedError = 0,
Device = CANTalon.FeedbackDevice.CtreMagEncoderRelative,
NominalVoltage = 0.0f,
PeakVoltage = 12.0f, //3.4f
PIDFValues = new PIDF
{
kP = 0.151,
kI = 0,
kD = 0,
kF = 0
},
ReverseSensor = true
});
Right1.SetEncoderPostition(0); // Reset the encoder position to 0.
Right1.ReverseOutput(true); // Reverse the direction of the motor.
Right2.MotorControlMode = WPILib.Interfaces.ControlMode.Follower;
Right2.Set(Right1.DeviceId); // Configure the second right talon to be a follower of the other right talon.
Left1.SetEncoderPostition(0); // Reset the eoncoder position to 0.
Left1.ReverseOutput(false); // Reverse the direction of the motor.
Left1.ReverseSensor(true);
Left2.MotorControlMode = WPILib.Interfaces.ControlMode.Follower;
Left2.Set(Left1.DeviceId); // Configure the second left talon to be a follower of the other left talon.
NavX.Reset(); // Reset the NavX's yaw value to zero.
}
/// <summary>
/// Runs the auto handler.
/// </summary>
protected override void Main()
{
Right1.SetEncoderPostition(0); // Reset the encoder position to 0.
Left1.SetEncoderPostition(0); // Reset the eoncoder position to 0.
// Runs the selected autonomous.
//AutoHandler.RunAuto(Handler.AutoPosition.Disabled);
// Both down. 0b00
if (DriveStick_Left.GetAxis(WPILib.Joystick.AxisType.Z) < 0.5 && DriveStick_Right.GetAxis(WPILib.Joystick.AxisType.Z) < 0.5)
{
SmartConsole.PrintInfo("Currently running disabled autonomous on the " + DriverStation.Instance.GetAlliance() + " alliance.");
AutoHandler.RunAuto(Handler.AutoPosition.Disabled);
}
// Left stick down, right stick up. 0b01
else if (DriveStick_Left.GetAxis(WPILib.Joystick.AxisType.Z) < 0.5 && DriveStick_Right.GetAxis(WPILib.Joystick.AxisType.Z) > 0.5)
{
SmartConsole.PrintInfo("Currently running position 1 autonomous on the " + DriverStation.Instance.GetAlliance() + " alliance.");
AutoHandler.RunAuto(Handler.AutoPosition.Position1);
}
// Left stick up, right stick down. 0b10
else if (DriveStick_Left.GetAxis(WPILib.Joystick.AxisType.Z) > 0.5 && DriveStick_Right.GetAxis(WPILib.Joystick.AxisType.Z) < 0.5)
{
SmartConsole.PrintInfo("Currently running position 2 autonomous on the " + DriverStation.Instance.GetAlliance() + " alliance.");
AutoHandler.RunAuto(Handler.AutoPosition.Position2);
/*DriveTrain.Move(.65, .70);
*
* Snooze(2500);
*
* DriveTrain.StopMotor();
*
* GearSlot.Set(true);
*
* while (true) Snooze(1);*/
}
// Left stick up, right stick up. 0b11
else if (DriveStick_Left.GetAxis(WPILib.Joystick.AxisType.Z) > 0.5 && DriveStick_Right.GetAxis(WPILib.Joystick.AxisType.Z) > 0.5)
{
SmartConsole.PrintInfo("Currently running position 3 autonomous on the " + DriverStation.Instance.GetAlliance() + " alliance.");
AutoHandler.RunAuto(Handler.AutoPosition.Position3);
}
}