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.commands.driving.DriveDistance;
   4 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
   5 import org.usfirst.frc.team3501.robot.subsystems.Intake;
   6 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
   7
   8 import edu.wpi.first.wpilibj.IterativeRobot;
   9 import edu.wpi.first.wpilibj.command.Scheduler;
  10 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  11
  12 public class Robot extends IterativeRobot {
  13   private static DriveTrain driveTrain;
  14   private static Shooter shooter;
  15   private static OI oi;
  16   private static Intake intake;
  17
  18   @Override
  19   public void robotInit() {
  20     driveTrain = DriveTrain.getDriveTrain();
  21     oi = OI.getOI();
  22     shooter = Shooter.getShooter();
  23     intake = Intake.getIntake();
  24   }
  25
  26   public static DriveTrain getDriveTrain() {
  27     return DriveTrain.getDriveTrain();
  28   }
  29
  30   public static Shooter getShooter() {
  31     return Shooter.getShooter();
  32   }
  33
  34   public static OI getOI() {
  35     return OI.getOI();
  36   }
  37
  38   public static Intake getIntake() {
  39     return Intake.getIntake();
  40   }
  41
  42   // If the gear values do not match in the left and right piston, then they are
  43   // both set to high gear
  44   @Override
  45   public void autonomousInit() {
  46     driveTrain.setHighGear();
  47
  48     SmartDashboard.putNumber(Constants.DriveTrain.P_Val, -1);
  49     SmartDashboard.putNumber(Constants.DriveTrain.I_Val, -1);
  50     SmartDashboard.putNumber(Constants.DriveTrain.D_Val, -1);
  51     SmartDashboard.putNumber(Constants.DriveTrain.TARGET_DIST, 50);
  52
  53   }
  54
  55   @Override
  56   public void autonomousPeriodic() {
  57     Scheduler.getInstance().run();
  58
  59     double P = SmartDashboard.getNumber(Constants.DriveTrain.P_Val, -1);
  60     double I = SmartDashboard.getNumber(Constants.DriveTrain.I_Val, -1);
  61     double D = SmartDashboard.getNumber(Constants.DriveTrain.D_Val, -1);
  62
  63     double SETPOINT = SmartDashboard.getNumber(Constants.DriveTrain.TARGET_DIST,
  64         -1);
  65
  66     DriveTrain.getDriveTrain().getDriveController().setConstants(P, I, D);
  67
  68     new DriveDistance(SETPOINT, 0.5).start();
  69   }
  70
  71   @Override
  72   public void teleopInit() {
  73
  74   }
  75
  76   @Override
  77   public void teleopPeriodic() {
  78     Scheduler.getInstance().run();
  79   }
  80 }