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