/// <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.
}
public static void DriveStraight(double time)
{
double initAngle = NavX.GetYaw();
Stopwatch sw = new Stopwatch();
sw.Start();
while (sw.ElapsedMilliseconds < time)
{
//double angle = RobotMap.NavX.GetYaw();
//double offset = angle - initAngle;
//if (offset < 0)
//{
// RobotMap.Left1.Set(0.75 + (0.05 * offset));
// Right1.Set(0.75);
//}
//else if (offset > 0)
//{
// Left1.Set(0.75);
// Right1.Set(0.75 + (0.05 * offset));
//}
//else
RobotMap.DriveTrain.SetLeftRightMotorOutputs(0.75, 0.75);
}
}
