1 package org
.usfirst
.frc
.team3501
.robot
;
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
;
8 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
9 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
10 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
12 public class Robot
extends IterativeRobot
{
13 private static DriveTrain driveTrain
;
14 private static Shooter shooter
;
16 private static Intake intake
;
19 public void robotInit() {
20 driveTrain
= DriveTrain
.getDriveTrain();
22 shooter
= Shooter
.getShooter();
23 intake
= Intake
.getIntake();
26 public static DriveTrain
getDriveTrain() {
27 return DriveTrain
.getDriveTrain();
30 public static Shooter
getShooter() {
31 return Shooter
.getShooter();
34 public static OI
getOI() {
38 public static Intake
getIntake() {
39 return Intake
.getIntake();
42 // If the gear values do not match in the left and right piston, then they are
43 // both set to high gear
45 public void autonomousInit() {
46 driveTrain
.setHighGear();
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
.MOTOR_VAL
, 0.5);
57 public void autonomousPeriodic() {
58 Scheduler
.getInstance().run();
60 double P
= SmartDashboard
.getNumber(Constants
.DriveTrain
.DRIVE_D_Val
,
61 Constants
.DriveTrain
.PID_ERROR
);
62 double I
= SmartDashboard
.getNumber(Constants
.DriveTrain
.DRIVE_I_Val
,
63 Constants
.DriveTrain
.PID_ERROR
);
64 double D
= SmartDashboard
.getNumber(Constants
.DriveTrain
.DRIVE_D_Val
,
65 Constants
.DriveTrain
.PID_ERROR
);
66 double SPEED
= SmartDashboard
.getNumber(Constants
.DriveTrain
.MOTOR_VAL
, 0);
68 double SETPOINT
= SmartDashboard
.getNumber(
69 Constants
.DriveTrain
.DRIVE_TARGET_DIST
,
70 Constants
.DriveTrain
.TARGET_DISTANCE_ERROR
);
72 DriveTrain
.getDriveTrain().getDriveController().setConstants(P
, I
, D
);
74 new DriveDistance(SETPOINT
, SPEED
).start();
78 public void teleopInit() {
83 public void teleopPeriodic() {
84 Scheduler
.getInstance().run();