package org.usfirst.frc.team3501.robot;
import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
-import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm;
-import org.usfirst.frc.team3501.robot.commands.shooter.ResetCatapult;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Robot extends IterativeRobot {
public static OI oi;
public static Shooter shooter;
public static IntakeArm intakeArm;
+ SendableChooser frontChooser;
+ boolean isFront;
+
+ CameraServer cameraServer;
+
@Override
public void robotInit() {
driveTrain = new DriveTrain();
- oi = new OI();
-
shooter = new Shooter();
intakeArm = new IntakeArm();
+
+ oi = new OI();
+ isFront = true;
+
+ frontChooser = new SendableChooser();
+ frontChooser.addDefault("Intake is front", false);
+ frontChooser.addObject("Shooter is front", true);
+
+ SmartDashboard.putData("Front chooser", frontChooser);
+
+ cameraServer = CameraServer.getInstance();
+ cameraServer.setQuality(50);
+ cameraServer.startAutomaticCapture();
+
}
@Override
public void autonomousInit() {
- Scheduler.getInstance().run();
+ // get options chosen from drop down menu
+
+ isFront = (boolean) frontChooser.getSelected();
+
+ if (!isFront)
+ driveTrain.toggleFlipped();
+
+ Scheduler.getInstance().add(new TimeDrive(.6, 4));
}
@Override
public void autonomousPeriodic() {
+
Scheduler.getInstance().run();
}
@Override
public void teleopInit() {
- Scheduler.getInstance().add(new SetLowGear()); // Start each match in low
- // gear
- Scheduler.getInstance().add(new ResetCatapult()); // Reset catapult at start
- // of each match.
-
- Scheduler.getInstance().add(new MoveIntakeArm(Constants.IntakeArm.EXTEND));
- // Start testing with intake arm extended TODO remove this
+ if (!isFront)
+ driveTrain.toggleFlipped();
+ Scheduler.getInstance().add(new SetLowGear());
}
@Override