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.subsystems.DriveTrain;
   4 import org.usfirst.frc.team3501.robot.subsystems.Intake;
   5 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
   6
   7 import edu.wpi.first.wpilibj.IterativeRobot;
   8 import edu.wpi.first.wpilibj.command.Scheduler;
   9 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  10
  11 public class Robot extends IterativeRobot {
  12   private static DriveTrain driveTrain;
  13   private static Shooter shooter;
  14   private static OI oi;
  15   private static Intake intake;
  16
  17   @Override
  18   public void robotInit() {
  19     driveTrain = DriveTrain.getDriveTrain();
  20     oi = OI.getOI();
  21     shooter = Shooter.getShooter();
  22     intake = Intake.getIntake();
  23   }
  24
  25   public static DriveTrain getDriveTrain() {
  26     return driveTrain;
  27   }
  28
  29   public static Shooter getShooter() {
  30     return Shooter.getShooter();
  31   }
  32
  33   public static OI getOI() {
  34     return OI.getOI();
  35   }
  36
  37   public static Intake getIntake() {
  38     return Intake.getIntake();
  39   }
  40
  41   // If the gear values do not match in the left and right piston, then they are
  42   // both set to high gear
  43   @Override
  44   public void autonomousInit() {
  45     driveTrain.setHighGear();
  46
  47 <<<<<<< 7361ba4ac28673251bd93f8a926927ca81a1eec6
  48     SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_P_Val, 0);
  49     SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_I_Val, 0);
  50     SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_D_Val, 0);
  51     SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_TARGET_DIST, 50);
  52     SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_MOTOR_VAL, 0.5);
  53
  54     SmartDashboard.putNumber(Constants.DriveTrain.GYRO_P_Val, 0);
  55     SmartDashboard.putNumber(Constants.DriveTrain.GYRO_I_Val, 0);
  56     SmartDashboard.putNumber(Constants.DriveTrain.GYRO_D_Val, 0);
  57
  58     double driveP = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
  59         Constants.DriveTrain.PID_ERROR);
  60     double driveI = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_I_Val,
  61         Constants.DriveTrain.PID_ERROR);
  62     double driveD = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
  63         Constants.DriveTrain.PID_ERROR);
  64
  65     double gyroP = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_P_Val,
  66         Constants.DriveTrain.PID_ERROR);
  67     double gyroI = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_I_Val,
  68         Constants.DriveTrain.PID_ERROR);
  69     double gyroD = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_D_Val,
  70         Constants.DriveTrain.PID_ERROR);
  71
  72     double Setpoint = SmartDashboard.getNumber(
  73         Constants.DriveTrain.DRIVE_TARGET_DIST, Constants.DriveTrain.PID_ERROR);
  74     double Speed = SmartDashboard.getNumber(
  75         Constants.DriveTrain.DRIVE_MOTOR_VAL, Constants.DriveTrain.PID_ERROR);
  76
  77     driveTrain.getDriveController().setName("Drive");
  78     driveTrain.getGyroController().setName("Gyro");
  79
  80     driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
  81
  82     driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
  83
  84   }
  85
  86   @Override
  87   public void autonomousPeriodic() {
  88     Scheduler.getInstance().run();
  89
  90     double driveP = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
  91         Constants.DriveTrain.PID_ERROR);
  92     double driveI = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_I_Val,
  93         Constants.DriveTrain.PID_ERROR);
  94     double driveD = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
  95         Constants.DriveTrain.PID_ERROR);
  96
  97     double gyroP = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_P_Val,
  98         Constants.DriveTrain.PID_ERROR);
  99     double gyroI = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_I_Val,
 100         Constants.DriveTrain.PID_ERROR);
 101     double gyroD = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_D_Val,
 102         Constants.DriveTrain.PID_ERROR);
 103
 104     driveTrain.getDriveController().setName("Drive");
 105     driveTrain.getGyroController().setName("Gyro");
 106
 107     driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
 108
 109     driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
 110
 111     // new DriveDistance(SETPOINT, SPEED).start();
 112
 113     // new TurnForAngle(0, Direction.FORWARD, 5).start();
 114   }
 115
 116   @Override
 117   public void teleopInit() {
 118
 119   }
 120
 121   @Override
 122   public void teleopPeriodic() {
 123     Scheduler.getInstance().run();
 124   }
 125 }