-
Notifications
You must be signed in to change notification settings - Fork 0
/
FRC2017c.cs
82 lines (71 loc) · 2.66 KB
/
FRC2017c.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
using System;
using System.Collections.Generic;
using System.Linq;
using WPILib;
using WPILib.Commands;
using CSCore;
using FRC2017c.Subsystems;
using FRC2017c.Commands;
namespace FRC2017c{
public class FRC2017c:IterativeRobot{
public static readonly PowerSubsystem powerSys=new PowerSubsystem();
public static readonly DrivingSubsystem driveSys=new DrivingSubsystem();
public static readonly OperatingSubsystem operateSys=new OperatingSubsystem();
public static readonly GyroSubsystem gyroSys=new GyroSubsystem();
public static readonly CommunicationSubsystem commSys=new CommunicationSubsystem();
public static OI oi;
public static NetworkTables.NetworkTable nt;
public string mode;
Command autonomousCommand;
WPILib.SmartDashboard.SendableChooser chooser;
// CameraServices
UsbCamera usbCamera;
CameraServer camServer;
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);
}
public override void DisabledInit(){
mode="disabled";
}
public override void DisabledPeriodic(){
Scheduler.Instance.Run();
FRC2017c.commSys.updateAll();
}
public override void AutonomousInit(){
autonomousCommand=(Command)chooser.GetSelected();
autonomousCommand.Start();
}
public override void AutonomousPeriodic(){
Scheduler.Instance.Run();
FRC2017c.commSys.updateAll();
}
public override void TeleopInit(){
FRC2017c.gyroSys.resetDisplacement();
}
public override void TeleopPeriodic(){
Scheduler.Instance.Run();
//System.Console.WriteLine(gyroSys.getDisplacement().ToString()+" X: "+gyroSys.getDisplacementX().ToString()+" Y: "+gyroSys.getDisplacementY().ToString()+" Z:"+gyroSys.getDisplacementZ().ToString());
FRC2017c.commSys.updateAll();
}
public override void TestPeriodic(){
WPILib.LiveWindow.LiveWindow.Run();
FRC2017c.commSys.updateAll();
}
}
}