public void Update()
{
double totalVoltage = Panel.GetVoltage();
SmartDashboard.PutNumber("Total Voltage", totalVoltage);
double totalCurrent = Panel.GetTotalCurrent();
SmartDashboard.PutNumber("Total Current", totalCurrent);
double totalPower = Panel.GetTotalPower();
SmartDashboard.PutNumber("Total Power", totalPower);
double totalEnergy = Panel.GetTotalEnergy();
SmartDashboard.PutNumber("Total Energy", totalEnergy);
double compressorCurrent = Compressor.GetCompressorCurrent();
SmartDashboard.PutNumber("Compressor Current", compressorCurrent);
foreach (PowerChannel channel in Channels)
{
int port = channel.Channel;
string name = channel.ChannelName;
double current = Panel.GetCurrent(port);
string ID = $"#{port} - {name}";
SmartDashboard.PutNumber(ID, current);
}
}
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 override void AutonomousPeriodic()
{
BlockArray blocks = new BlockArray(2);
SmartDashboard.PutNumber("Pixy Count", Pixy.GetBlocks(2, blocks));
SmartDashboard.PutNumber("Pixy Area", blocks.getitem(0).height *blocks.getitem(0).width);
}
/// <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 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 dashboardInit()
{
incrementDashboardLoop();
if (dashboardLoop != 0)
{
return;
}
SmartDashboard.PutNumber("autoChoice", 0);
SmartDashboard.PutNumber("Moat", 0);
}
private void AutoLerp(Joystick stick, double amount)
{
leftX = Lerp(leftX, stick.GetRawAxis(XboxMap.LeftX), amount);
leftY = Lerp(leftY, stick.GetRawAxis(XboxMap.LeftY), amount);
rightX = Lerp(rightX, stick.GetRawAxis(XboxMap.RightX), amount);
rightY = Lerp(rightY, stick.GetRawAxis(XboxMap.RightY), amount);
SmartDashboard.PutNumber("Left X", leftX);
SmartDashboard.PutNumber("Left Y", leftY);
SmartDashboard.PutNumber("Right X", rightX);
SmartDashboard.PutNumber("Right Y", rightY);
}
/// <summary>
/// This function is called periodically during operator control
/// </summary>
public override void TeleopPeriodic()
{
//while teleop is enabled, and the drivestation is enabled, run this code
while (IsOperatorControl && IsEnabled)
{
//clear out values for new run
SmartDashboard.PutString("DB/String 3", "");
if (stick.GetRawButton(5))
{
drive.TankDrive(stick.GetRawAxis(5), stick.GetRawAxis(1));
}
else
{
if (!stick.GetRawButton(6))
{
drive.TankDrive(stick.GetRawAxis(5) / 1.3, stick.GetRawAxis(1) / 1.3);
}
else
{
drive.TankDrive(-stick.GetRawAxis(1) / 1.3, -stick.GetRawAxis(5) / 1.3);
}
}
SmartDashboard.PutNumber("DB/String 0", stick.GetRawAxis(5));
SmartDashboard.PutNumber("DB/String 1", stick.GetRawAxis(1));
//stick.GetRawButton(0) should be the "a" button
//GetRawButton(1) should be "b"
double speed = 0.0;
// if (stick.GetRawButton(8))
//{
if (stick.GetRawButton(1) || stick.GetRawButton(2))
{
speed += (stick.GetRawButton(1) ? 0.5 : 0.0);
speed += (stick.GetRawButton(2) ? 0.5 : 0.0);
}
//}
climber.SetSpeed(-speed);
getRope.SetSpeed(stick.GetRawAxis(2) / 2 - stick.GetRawAxis(3) / 2);
//create a delay of .1 second
Timer.Delay(0.1);
}
}
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 override unsafe void RobotPeriodic()
{
SmartDashboard.PutNumber("External Dir Tach", externalDirectionCounter.Count);
SmartDashboard.PutNumber("External Dir Tach 2x", externalDirectionCounter2x.Count);
SmartDashboard.PutNumber("Absolute Tach", absoluteTach.RotationalSpeed?.RevolutionsPerSecond ?? double.MaxValue);
SmartDashboard.PutNumber("Quadrature Tach", quadratureTach.RotationalSpeed?.RevolutionsPerSecond ?? 0);
SmartDashboard.PutNumber("Quadrature Counter", quadratureCounter.Count);
SmartDashboard.PutNumber("Joystick", DriverStation.Instance.GetStickAxis(0, 1));
sparkMax.Set(DriverStation.Instance.GetStickAxis(0, 1));
if (DriverStation.Instance.GetStickButton(0, 1))
{
Console.WriteLine("Disable");
interrupt.Disable();
}
if (DriverStation.Instance.GetStickButton(0, 2))
{
Console.WriteLine("Enable");
interrupt.Enable();
}
//sparkMax.SetVoltage(ElectricPotential.FromVolts(5));
//int idxLocal = idx;
//try
//{
// CANAPI.WritePacket(can, new Span<byte>(&idxLocal, 4), 42);
//}
//catch (UncleanStatusException ex)
//{
// ;
//}
//var current = Timer.FPGATimestamp;
//var delta = current - lastTime;
//lastTime = current;
//buffer[idx] = delta.TotalMilliseconds;
//idx++;
//if (idx == 50)
//{
// Console.WriteLine(buffer.Average());
// idx = 0;
//}
//base.RobotPeriodic();
}
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);
}
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 void RunAuto(AutoPosition position)
{
SmartDashboard.PutNumber("Hello World!", 123);
IAuto auto;
switch (GetAuto(position))
{
case AutoMode.Disabled:
auto = new Disabled();
break;
case AutoMode.Red1:
auto = new Red1();
break;
case AutoMode.Red2:
auto = new Red2();
break;
case AutoMode.Red3:
auto = new Red3();
break;
case AutoMode.Blue1:
auto = new Blue1();
break;
case AutoMode.Blue2:
auto = new Blue2();
break;
case AutoMode.Blue3:
auto = new Blue3();
break;
default:
auto = new Disabled();
break;
}
SmartConsole.PrintInfo("You are currently running " + auto.GetType().Name + ". Have a great match! :)");
auto.Run();
}
public override void TeleopPeriodic()
{
Scheduler.Instance.Run();
Joystick stick = Oi.Pilot;
//train._drive.ArcadeDrive(1, 1);
stick.SetRumble(RumbleType.LeftRumble, stick.GetRawAxis(XboxMap.LeftTrigger));
stick.SetRumble(RumbleType.RightRumble, stick.GetRawAxis(XboxMap.RightTrigger));
SmartDashboard.PutNumber("POV", Oi.Pilot.GetPOV(0));
SmartDashboard.PutString("Drive Mode", Oi.DriveStyle.ToString());
SmartDashboard.PutString("Gear", Oi.ShifterGear.ToString());
SmartDashboard.PutData("Compressor On", new CompressorOnCommand());
SmartDashboard.PutData("Compressor Off", new CompressorOffCommand());
Air.Update();
Match.Update();
Power.Update();
}
public void Update()
{
SmartDashboard.PutNumber("Analog Pressure Voltage", Pressure.GetVoltage());
SmartDashboard.PutNumber("~Pressure (PSI)", AnalogToUnitPSI(Pressure.GetVoltage()));
}
public static bool OnTarget()
{
SmartDashboard.PutNumber("Left Difference", Math.Abs(-RobotMap.Left1.GetEncoderPosition() / 4096 - RobotMap.Left1.Setpoint));
SmartDashboard.PutNumber("Right Difference", Math.Abs(RobotMap.Right1.Setpoint - RobotMap.Right1.GetEncoderPosition() / 4096));
return(Math.Abs(-RobotMap.Left1.GetEncoderPosition() / 4096 - RobotMap.Left1.Setpoint) > 0.9 || Math.Abs(RobotMap.Right1.Setpoint - RobotMap.Right1.GetEncoderPosition() / 4096) > 0.9);
}
protected override void Main()
{
LiveWindow.Run();
SmartDashboard.PutNumber("NavX GetYaw", RobotMap.NavX.GetYaw());
}
public override void RobotPeriodic()
{
//Console.WriteLine("Getting Gyro");
SmartDashboard.PutNumber("Gyro", gyro.GetAngle());
}
public override void DisabledPeriodic()
{
SmartDashboard.PutNumber("DisabledCount", count++);
}