challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/Robot.java

   1 package org.usfirst.frc.team3501.robot;
   2
   3 import org.usfirst.frc.team3501.robot.commandgroups.AutonMiddleGear;
   4 import org.usfirst.frc.team3501.robot.commandgroups.AutonSideGear;
   5 import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
   6 import org.usfirst.frc.team3501.robot.subsystems.Climber;
   7 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
   8 import org.usfirst.frc.team3501.robot.subsystems.Intake;
   9 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
  10
  11 import edu.wpi.cscore.UsbCamera;
  12 import edu.wpi.first.wpilibj.CameraServer;
  13 import edu.wpi.first.wpilibj.DriverStation;
  14 import edu.wpi.first.wpilibj.IterativeRobot;
  15 import edu.wpi.first.wpilibj.command.Command;
  16 import edu.wpi.first.wpilibj.command.Scheduler;
  17 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  18 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  19
  20 public class Robot extends IterativeRobot {
  21   private static DriveTrain driveTrain;
  22   private static Shooter shooter;
  23   private static OI oi;
  24   private static Intake intake;
  25   private static Climber climber;
  26
  27   Command autonCommand;
  28   SendableChooser autonChooser;
  29
  30   @Override
  31   public void robotInit() {
  32     driveTrain = DriveTrain.getDriveTrain();
  33     oi = OI.getOI();
  34     shooter = Shooter.getShooter();
  35     intake = Intake.getIntake();
  36     climber = Climber.getClimber();
  37
  38     autonChooser = new SendableChooser();
  39     autonChooser.addDefault("Middle Gear", new AutonMiddleGear());
  40     autonChooser.addObject("Red Boiler Gear",
  41         new AutonSideGear("RED", "BOILER"));
  42     autonChooser.addObject("Red Retrieval Gear",
  43         new AutonSideGear("RED", "RETRIEVAL"));
  44     autonChooser.addObject("Blue Boiler Gear",
  45         new AutonSideGear("BLUE", "BOILER"));
  46     autonChooser.addObject("Blue Retrieval Gear",
  47         new AutonSideGear("BLUE", "RETRIEVAL"));
  48     SmartDashboard.putData("Autonomous Chooser", autonChooser);
  49
  50     CameraServer server = CameraServer.getInstance();
  51     UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0);
  52     UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1);
  53   }
  54
  55   public static DriveTrain getDriveTrain() {
  56     return DriveTrain.getDriveTrain();
  57   }
  58
  59   public static Shooter getShooter() {
  60     return Shooter.getShooter();
  61   }
  62
  63   public static OI getOI() {
  64     return OI.getOI();
  65   }
  66
  67   public static Intake getIntake() {
  68     return Intake.getIntake();
  69   }
  70
  71   public static Climber getClimber() {
  72     return Climber.getClimber();
  73   }
  74
  75   @Override
  76   public void autonomousInit() {
  77     driveTrain.setLowGear();
  78
  79     // autonCommand = (Command) autonChooser.getSelected();
  80     autonCommand = new TimeDrive(2, 0.6);
  81     Scheduler.getInstance().add(autonCommand);
  82   }
  83
  84   @Override
  85   public void autonomousPeriodic() {
  86     Scheduler.getInstance().run();
  87   }
  88
  89   @Override
  90   public void teleopInit() {
  91     driveTrain.setHighGear();
  92   }
  93
  94   @Override
  95   public void teleopPeriodic() {
  96     Scheduler.getInstance().run();
  97     updateSmartDashboard();
  98   }
  99
 100   public void updateSmartDashboard() {
 101     SmartDashboard.putNumber("left encode ",
 102         driveTrain.getLeftEncoderDistance());
 103     SmartDashboard.putNumber("right encoder",
 104         driveTrain.getRightEncoderDistance());
 105     SmartDashboard.putNumber("angle", driveTrain.getAngle());
 106     SmartDashboard.putNumber("voltage",
 107         DriverStation.getInstance().getBatteryVoltage());
 108     SmartDashboard.putNumber("rpm", shooter.getShooterRPM());
 109     SmartDashboard.putNumber("target shooting",
 110         shooter.getTargetShootingSpeed());
 111   }
 112 }