public override void AutonomousInit()
{
SmartDashboard.PutBoolean("Pixy Status", Pixy.IsConnected);
BlockArray blocks = new BlockArray(2);
SmartDashboard.PutNumber("Pixy Count", Pixy.GetBlocks(2, blocks));
}
public void dashboardTeleop()
{
incrementDashboardLoop();
if (dashboardLoop != 0)
{
return;
}
SmartDashboard.PutNumber("speedA", 60.0 / shooterSpeedA.GetPeriod());
SmartDashboard.PutNumber("speedB", 60.0 / shooterSpeedB.GetPeriod());
SmartDashboard.PutBoolean("ballSensor", !ballSensor.Get());
SmartDashboard.PutBoolean("tapeSensor", tapeSensor.Get());
SmartDashboard.PutNumber("armPot", armPot.GetAverageVoltage());
SmartDashboard.PutNumber("shooterAngle", shooterPot.GetVoltage());
SmartDashboard.PutNumber("distL", leftDriveEncoder.GetRaw());
SmartDashboard.PutNumber("distR", rightDriveEncoder.GetRaw());
SmartDashboard.PutBoolean("armLimit", armLowerLimit.Get());
SmartDashboard.PutBoolean("armUpLimit", armUpperLimit.Get());
SmartDashboard.PutNumber("yaw", navx.GetYaw());
SmartDashboard.PutNumber("pitch", navx.GetPitch());
SmartDashboard.PutNumber("roll", navx.GetRoll());
SmartDashboard.PutBoolean("navXenabled", navx.IsConnected());
SmartDashboard.PutNumber("sonar", sonar.GetAverageVoltage());
/*
* Variables I haven't set yet
* SmartDashboard.PutNumber("powerA", powerA);
* SmartDashboard.PutNumber("powerB", powerB);
* SmartDashboard.PutBoolean("foundTarget", foundTarget);
*/
}
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 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);
}
/// <summary>
/// Drive to the desired angle.
/// </summary>
/// <param name="desiredAngle">Desired angle from -180 to 180.</param>
public static void TurnToAngle(double desiredAngle)
{
Left1.MotorControlMode = WPILib.Interfaces.ControlMode.PercentVbus;
Right1.MotorControlMode = WPILib.Interfaces.ControlMode.PercentVbus;
// Set the controller setpoint to the desired angle.
TurnController.Controller.Setpoint = desiredAngle;
// Enables the controller.
TurnController.Controller.Enable();
// Keeps the robot in a loop until it is on target.
while (TurnController.Controller.OnTarget() == false)
{
SmartDashboard.PutBoolean("On Target?", TurnController.Controller.OnTarget());
}
SmartDashboard.PutBoolean("On Target?", TurnController.Controller.OnTarget());
// Disables the controller since we are on target.
RobotMap.TurnController.Controller.Disable();
}
public void dashboardDisabled()
{
incrementDashboardLoop();
if (dashboardLoop != 0)
{
return;
}
SmartDashboard.PutNumber("speedA", 60.0 / shooterSpeedA.GetPeriod());
SmartDashboard.PutNumber("speedB", 60.0 / shooterSpeedB.GetPeriod());
SmartDashboard.PutBoolean("ballSensor", !ballSensor.Get());
SmartDashboard.PutBoolean("tapeSensor", tapeSensor.Get());
SmartDashboard.PutNumber("armPot", armPot.GetAverageVoltage());
SmartDashboard.PutNumber("shooterAngle", shooterPot.GetVoltage());
SmartDashboard.PutNumber("distL", leftDriveEncoder.GetRaw());
SmartDashboard.PutNumber("distR", rightDriveEncoder.GetRaw());
SmartDashboard.PutBoolean("armLimit", armLowerLimit.Get());
SmartDashboard.PutBoolean("armUpLimit", armUpperLimit.Get());
SmartDashboard.PutNumber("yaw", navx.GetYaw());
SmartDashboard.PutNumber("pitch", navx.GetPitch());
SmartDashboard.PutNumber("roll", navx.GetRoll());
SmartDashboard.PutBoolean("navXenabled", navx.IsConnected());
SmartDashboard.PutNumber("sonar", sonar.GetAverageVoltage());
SmartDashboard.PutBoolean("shooterOn", false);
/*
* Variables I haven't set yet
* SmartDashboard.PutNumber("powerA", powerA);
* SmartDashboard.PutNumber("powerB", powerB);
* SmartDashboard.PutBoolean("foundTarget", foundTarget);
* SmartDashboard.PutNumber("desiredSpeedA", setSpeedA);
* SmartDashboard.PutNumber("desiredSpeedB", setSpeedB);
* SmartDashboard.PutNumber("minRoll", minRoll);
* SmartDashboard.PutNumber("maxRoll", maxRoll);
* SmartDashboard.PutNumber("lastAutoStep", autoStep);
* SmartDashboard.PutNumber("defenseDistance", defenseDist);
* SmartDashboard.PutNumber("seeTapeDist", seeTapeDist);
* SmartDashboard.PutNumber("wallDist", wallDist);
* SmartDashboard.PutBoolean("onSpeed", onSpeed);
* SmartDashboard.PutNumber("turnAngle", turnToAngle);
* SmartDashboard.PutNumber("autoChoice", autoRoutine);
* SmartDashboard.PutNumber("moat", moat);
*/
}
public bool OnTarget()
{
SmartDashboard.PutBoolean(this.GetType().Name.ToString() + "OnTarget", pidController.OnTarget());
return(pidController.OnTarget());
}
public override void TeleopPeriodic()
{
SmartDashboard.PutBoolean("Pixy Status", Pixy.IsConnected);
}
public override void RobotInit()
{
Pixy = new PixyCamera();
Pixy.Initialize();
SmartDashboard.PutBoolean("Pixy Status", Pixy.IsConnected);
}