public void Run()
{
//// Place the gear.
//AutoFunctions.Drive(1.6418); // Drive forward 1.6418 meters.
//AutoFunctions.TurnToAngle(-30); // Turn to -30 degrees.
//AutoFunctions.Drive(0.96); // Drive forward 0.96 meters to align at gear peg.
//AutoFunctions.PlaceGear(); // Place the gear. This might incorporate vision. Currently does not.
//// Drive to the boiler and shoot.
//AutoFunctions.Drive(-0.96); // Drive 0.96 meters backwards away from gear peg.
//AutoFunctions.TurnToAngle(0); // Turn to 0 degrees.
//AutoFunctions.Drive(-0.7); // Drive backwards -0.7 meters to clear airship for driving.
//AutoFunctions.TurnToAngle(133.74); // Turn to 133.74 degrees.
//AutoFunctions.Drive(6.5); // Drive 6.5 meters forward to the boiler. NEEDS CLARIFICATION FROM LOGAN.
//AutoFunctions.ShootFuel(); // Shoot fuel. This might incorporate vision. Currently does not.
//// Drive to a hopper.
//AutoFunctions.Drive(-2.1); // Back out of the boiler by driving back 2.1 meters.
//AutoFunctions.TurnToAngle(33.03); // Turn to 33.03 degrees.
//AutoFunctions.Drive(1.5); // Drive forward 1.5 meteres to the hopper.
//AutoFunctions.TurnToAngle(90); // Turn to 90 degrees, which is perpendicular to the perimeter.
//AutoFunctions.Drive(1.275); // Drive forward 1.275 meters to the hopper.
AutoFunctions.Drive(2.8);
Stopwatch stop = new Stopwatch();
stop.Start();
while (AutoFunctions.OnTarget() || stop.ElapsedMilliseconds < 8000)
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
RobotMap.GearSlot.Set(true);
AutoFunctions.DriveFast(0.35);
Stopwatch sw = new Stopwatch();
sw.Start();
while (sw.ElapsedMilliseconds < 5000)
{
;
}
AutoFunctions.Drive(-1.75);
while (AutoFunctions.OnTarget())
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
AutoFunctions.TurnToAngle(160);
}
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.
}
public void Run()
{
//AutoFunctions.DriveStraight(5000);
//RobotMap.GearSlot.Set(true);
// Place the gear.
//AutoFunctions.Drive(0.52); // Drive forward 0.52 meters.
//AutoFunctions.TurnToAngle(-32.70); // Turn to 60.25 degrees.
//AutoFunctions.Drive(0.72); // Drive forward 0.72 meters.
//AutoFunctions.TurnToAngle(0); // Turn to 0 degrees.
//AutoFunctions.Drive(1.00); // Drive forward one meter to place the gear.
//AutoFunctions.PlaceGear(); // Place the gear.
AutoFunctions.Drive(2.08);
Stopwatch sw = new Stopwatch();
while (AutoFunctions.OnTarget() || sw.ElapsedMilliseconds < 5000)
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
RobotMap.GearSlot.Set(true);
sw.Start();
while (sw.ElapsedMilliseconds < 5000)
{
;
}
AutoFunctions.Drive(-1.75);
while (AutoFunctions.OnTarget())
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
AutoFunctions.TurnToAngle(90);
AutoFunctions.Drive(3);
while (AutoFunctions.OnTarget())
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
}
public void Run()
{
RobotMap.Shooter.Set(.95);
Stopwatch sw = new Stopwatch();
sw.Start();
while (sw.ElapsedMilliseconds < 1000)
{
;
}
RobotMap.Agitator.Set(-1);
RobotMap.IntakeSecondStage.Set(-1);
sw.Reset();
sw.Start();
while (sw.ElapsedMilliseconds < 12000)
{
;
}
RobotMap.Shooter.Set(0);
RobotMap.Agitator.Set(0);
RobotMap.IntakeSecondStage.Set(0);
AutoFunctions.DriveFast(-3.1);
//// Place the gear.
//AutoFunctions.Drive(2.445); // Drive forward one meter to place the gear.
//AutoFunctions.TurnToAngle(60.25); // Turn to 60.25 degrees.
//AutoFunctions.Drive(1.175); // Drive forward 1.175 meters.
//AutoFunctions.PlaceGear(); // Place the gear.
//// Drive to a hopper.
//AutoFunctions.Drive(-1.175); // Drive backwards 1.175 meters.
//AutoFunctions.TurnToAngle(17.3); // Turn to 17.3 degrees.
//AutoFunctions.Drive(2.84); // Drive forwards 2.84 meters.
//AutoFunctions.TurnToAngle(-90); // Turn to -90 degrees.
//AutoFunctions.Drive(0.825); // Drive forward 0.825 to the hopper.
//AutoFunctions.Drive(3.08);
//while (AutoFunctions.OnTarget())
//{
// SmartDashboard.PutBoolean("In loop?", true);
// SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
// SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
//}
//SmartDashboard.PutBoolean("In loop?", false);
//RobotMap.GearSlot.Set(true);
//Stopwatch sw = new Stopwatch();
//sw.Start();
//while (sw.ElapsedMilliseconds < 5000) ;
//AutoFunctions.Drive(-1.75);
//while (AutoFunctions.OnTarget())
//{
// SmartDashboard.PutBoolean("In loop?", true);
// SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
// SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
//}
//SmartDashboard.PutBoolean("In loop?", false);
//AutoFunctions.TurnToAngle(60);
//AutoFunctions.Drive(5);
//while (AutoFunctions.OnTarget())
//{
// SmartDashboard.PutBoolean("In loop?", true);
// SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
// SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
//}
//SmartDashboard.PutBoolean("In loop?", false);
}
