private void GetReportButton_Click(object sender, RoutedEventArgs e)
{
NetworkTables.NetworkTable table = NetworkTables.NetworkTable.GetTable("CamData");
double[] heights = table.GetNumberArray("Height", new double[] { 0 });
foreach (double h in heights)
{
Console.WriteLine(h);
}
}
public override void RobotInit()
{
System.Console.WriteLine("Hello, FRC2017!");
System.Console.WriteLine("TrueMoe RobotCode 2017c");
oi = new OI();
FRC2017c.gyroSys.reset();
chooser = new WPILib.SmartDashboard.SendableChooser();
camServer = CameraServer.Instance;
nt = NetworkTables.NetworkTable.GetTable("Forgiving/Vision");
chooser.AddDefault("Center", new AutonomousCommand("center"));
chooser.AddObject("turn Right", new AutonomousCommand("left"));
chooser.AddObject("turn Left", new AutonomousCommand("right"));
WPILib.SmartDashboard.SmartDashboard.PutData("Autonomous Mode", chooser);
WPILib.SmartDashboard.SmartDashboard.PutString("Team", "5453");
WPILib.SmartDashboard.SmartDashboard.PutNumber("AutonomousRushing1Delay", 900);
FRC2017c.gyroSys.resetDisplacement();
usbCamera = new UsbCamera("USB Camera 0", 0);
usbCamera.SetFPS(12);
camServer.StartAutomaticCapture(usbCamera);
}
private void TakeSnapshot(object sender, RoutedEventArgs e)
{
//If a snapshot is already present
if (StaticResources.SnapshotEnabled)
{
Console.WriteLine("Snapshot already enabled. Adding another!");
contourDataAnalyzer.AddSnapshot();
}
//First-time
else
{
NetworkTables.NetworkTable table = NetworkTables.NetworkTable.GetTable("CamData");
double[] heights = table.GetNumberArray("Height", new double[] { 1 });
double[] widths = table.GetNumberArray("Width", new double[] { 1 });
try
{
StaticResources.ContourRatio.Add(widths[0] / heights[0]);
StaticResources.SnapshotEnabled = true;
TakeSnapButton.Content = "Add Snapshot";
}
catch (IndexOutOfRangeException) { MessageBox.Show("Index out of range. Please clear snapshot and try again."); }
}
}
protected override async void Execute()
{
nt = NetworkTables.NetworkTable.GetTable("Forgiving/Vision");
nt.PutString("turn", "null");
nt.PutNumber("angle", 0.0);
double angle;
switch (System.Math.Abs(robotLocation))
{
case 1:
System.Console.WriteLine("Initial Location set to " + ((robotLocation == -1)?"LEFT":"RIGHT"));
FRC2017c.operateSys.gearIntake(0.3);
//int test=0;
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed, 0, RobotMap.drivingSquaredInput);
int autonomousRushing1Delay = System.Convert.ToInt32(WPILib.SmartDashboard.SmartDashboard.GetNumber("AutonomousRushing1Delay", 900));
System.Threading.Thread.Sleep(autonomousRushing1Delay);
FRC2017c.operateSys.gearUp(-1);
System.Threading.Thread.Sleep(600);
//test++;System.Console.WriteLine("orz: "+test.ToString());
FRC2017c.driveSys.arcadeDrive(-0.3, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(340);
FRC2017c.operateSys.gearUp(0);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
nt.PutString("turn", "null");
nt.PutNumber("angle", 0.0);
await amazingAutoTurn(RobotMap.autonomousAutoGearAngle, -robotLocation, 1);
FRC2017c.operateSys.gearUp(-1);
FRC2017c.powerSys.stallDetectionDelay(RobotMap.pdpMotorGearUp, RobotMap.stallMotorGearUp);
System.Console.WriteLine("Stalled!");
FRC2017c.operateSys.gearUp(0);
System.Threading.Thread.Sleep(300);
FRC2017c.operateSys.gearUp(1);
System.Threading.Thread.Sleep(500);
FRC2017c.operateSys.gearUp(0);
/*
* FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed*0.7,0,RobotMap.drivingSquaredInput);
* System.Threading.Thread.Sleep(100);
* FRC2017c.driveSys.arcadeDrive(0,0,RobotMap.drivingSquaredInput);
* System.Threading.Thread.Sleep(500);
*/
System.Threading.Thread.Sleep(1080);
angle = nt.GetNumber("angle", 0.0);
if (System.Math.Abs(angle) < 0.1)
{
System.Threading.Thread.Sleep(300);
angle = nt.GetNumber("angle", 0.0);
}
await amazingAutoTurn(System.Math.Abs(angle), 0.57 *((nt.GetString("turn", "null") == "right") ? 1 : -1), 1);
System.Threading.Thread.Sleep(180);
FRC2017c.operateSys.gearUp(1);
System.Threading.Thread.Sleep(600);
FRC2017c.operateSys.gearUp(0);
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed * 0.64, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(950);
FRC2017c.driveSys.arcadeDrive(0.2, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(440);
FRC2017c.operateSys.gearIntake(-0.16);
System.Threading.Thread.Sleep(6000);
FRC2017c.operateSys.gearIntake(0);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
break;
case 0:
System.Console.WriteLine("Initial Location set to CENTER");
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed * 0.82, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(600);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
FRC2017c.operateSys.gearUp(1);
FRC2017c.powerSys.stallDetectionDelay(RobotMap.pdpMotorGearUp, RobotMap.stallMotorGearUpSmall);
System.Console.WriteLine("Stalled!");
FRC2017c.operateSys.gearUp(-1);
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed * 0.6, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(1000);
FRC2017c.operateSys.gearUp(0);
System.Threading.Thread.Sleep(860);
FRC2017c.driveSys.arcadeDrive(0.2, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(300);
FRC2017c.operateSys.gearIntake(-0.16);
System.Threading.Thread.Sleep(6000);
FRC2017c.operateSys.gearIntake(0);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
break;
}
}
