everything done except mapping buttons to actions
[ozzloy@gmail.com/3501-spark-go] / src / org / usfirst / frc / team3501 / robot / Robot.java
index 68770bb4819d35f1debc60c463d8c1aade4076d2..23a43b7812daab2e2bfe1d188ebfcb025f998b7c 100644 (file)
@@ -5,26 +5,36 @@ import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-import org.usfirst.frc.team3501.robot.commands.*;
+import org.usfirst.frc.team3501.robot.autons.*;
 import org.usfirst.frc.team3501.robot.subsystems.*;
 
 public class Robot extends IterativeRobot {
 
-       public static final Drivetrain drivetrain = new Drivetrain();
-       public static final Arm               arm = new Arm();
-       public static final Claw             claw = new Claw();
+       public static Drivetrain drivetrain;
+       public static Arm arm;
+       public static Claw claw;
+
+       public static Pneumatics pneumatics;
 
        public static OI oi;
 
-    Command autonomousCommand;
+       private SendableChooser autoChooser;
+
+    private Command autonomousCommand;
 
     public void robotInit() {
                oi = new OI();
 
-               double time = 1.2;
-               double speed = 0.7;
-        autonomousCommand = new DriveForward(time, speed);
+               drivetrain = new Drivetrain();
+               arm        = new Arm();
+               claw       = new Claw();
+
+               pneumatics = new Pneumatics();
+
+               chooseAuto();
     }
 
        public void disabledPeriodic() {
@@ -32,6 +42,9 @@ public class Robot extends IterativeRobot {
        }
 
     public void autonomousInit() {
+        pneumatics.start();
+
+        autonomousCommand = (Command) autoChooser.getSelected();
         autonomousCommand.start();
     }
 
@@ -40,16 +53,29 @@ public class Robot extends IterativeRobot {
     }
 
     public void teleopInit() {
+        pneumatics.start();
+
         autonomousCommand.cancel();
     }
 
     public void teleopPeriodic() {
         Scheduler.getInstance().run();
-
-
     }
 
     public void testPeriodic() {
         LiveWindow.run();
     }
+
+    public void disabledInit() {
+        pneumatics.stop();
+    }
+
+    private void chooseAuto() {
+        autoChooser = new SendableChooser();
+
+        autoChooser.addDefault("Drive over step", new DriveOverStep());
+        autoChooser.addObject("Drive past step",  new DrivePastStep());
+
+        SmartDashboard.putData("Auto Mode", autoChooser);
+    }
 }