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     SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0.006);
  26     SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0.0011);
  27     SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, -0.002);
  28     SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0.01);
  29
  30     SmartDashboard.putNumber(driveTrain.DRIVE_TARGET_DIST, 50);
  31     SmartDashboard.putNumber(driveTrain.MAX_TIME_OUT, 10);
  32
  33     SmartDashboard.putNumber(driveTrain.GYRO_P_Val, 0);
  34     SmartDashboard.putNumber(driveTrain.GYRO_I_Val, 0);
  35     SmartDashboard.putNumber(driveTrain.GYRO_D_Val, 0);
  36
  37   }
  38
  39   public static DriveTrain getDriveTrain() {
  40     return driveTrain;
  41   }
  42
  43   public static Shooter getShooter() {
  44     return Shooter.getShooter();
  45   }
  46
  47   public static OI getOI() {
  48     return OI.getOI();
  49   }
  50
  51   public static Intake getIntake() {
  52     return Intake.getIntake();
  53   }
  54
  55   // If the gear values do not match in the left and right piston, then they are
  56   // both set to high gear
  57   @Override
  58   public void autonomousInit() {
  59     System.out.println("AUTON INIT");
  60     driveTrain.setHighGear();
  61
  62     double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_P_Val,
  63         driveTrain.PID_ERROR);
  64     double driveI = SmartDashboard.getNumber(driveTrain.DRIVE_I_Val,
  65         driveTrain.PID_ERROR);
  66     double driveD = SmartDashboard.getNumber(driveTrain.DRIVE_D_Val,
  67         driveTrain.PID_ERROR);
  68
  69     double gyroP = SmartDashboard.getNumber(driveTrain.GYRO_P_Val,
  70         driveTrain.PID_ERROR);
  71     double gyroI = SmartDashboard.getNumber(driveTrain.GYRO_I_Val,
  72         driveTrain.PID_ERROR);
  73     double gyroD = SmartDashboard.getNumber(driveTrain.GYRO_D_Val,
  74         driveTrain.PID_ERROR);
  75
  76     double setpoint = SmartDashboard.getNumber(driveTrain.DRIVE_TARGET_DIST,
  77         driveTrain.PID_ERROR);
  78     double maxTimeOut = SmartDashboard.getNumber(driveTrain.MAX_TIME_OUT,
  79         driveTrain.PID_ERROR);
  80
  81     driveTrain.getDriveController().setName("Drive");
  82     driveTrain.getGyroController().setName("Gyro");
  83
  84     driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
  85     driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
  86
  87     Scheduler.getInstance()
  88         .add(new DriveDistance(setpoint, maxTimeOut));
  89   }
  90
  91   @Override
  92   public void autonomousPeriodic() {
  93     Scheduler.getInstance().run();
  94     SmartDashboard.putNumber("angle", driveTrain.getAngle());
  95   }
  96
  97   @Override
  98   public void teleopInit() {
  99
 100   }
 101
 102   @Override
 103   public void teleopPeriodic() {
 104     Scheduler.getInstance().run();
 105   }
 106 }