6388d0a171251eeebe101070816315715c7be208
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();
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_GYRO_P_Val
, 0);
30 SmartDashboard
.putNumber(driveTrain
.DRIVE_TARGET_DIST
, 50);
31 SmartDashboard
.putNumber(driveTrain
.MAX_TIME_OUT
, 10);
33 SmartDashboard
.putNumber(driveTrain
.GYRO_P_Val
, 0);
34 SmartDashboard
.putNumber(driveTrain
.GYRO_I_Val
, 0);
35 SmartDashboard
.putNumber(driveTrain
.GYRO_D_Val
, 0);
39 public static DriveTrain
getDriveTrain() {
43 public static Shooter
getShooter() {
44 return Shooter
.getShooter();
47 public static OI
getOI() {
51 public static Intake
getIntake() {
52 return Intake
.getIntake();
55 // If the gear values do not match in the left and right piston, then they are
56 // both set to high gear
58 public void autonomousInit() {
59 driveTrain
.setHighGear();
61 double driveP
= SmartDashboard
.getNumber(driveTrain
.DRIVE_D_Val
,
62 driveTrain
.PID_ERROR
);
63 double driveI
= SmartDashboard
.getNumber(driveTrain
.DRIVE_I_Val
,
64 driveTrain
.PID_ERROR
);
65 double driveD
= SmartDashboard
.getNumber(driveTrain
.DRIVE_D_Val
,
66 driveTrain
.PID_ERROR
);
68 double gyroP
= SmartDashboard
.getNumber(driveTrain
.GYRO_P_Val
,
69 driveTrain
.PID_ERROR
);
70 double gyroI
= SmartDashboard
.getNumber(driveTrain
.GYRO_I_Val
,
71 driveTrain
.PID_ERROR
);
72 double gyroD
= SmartDashboard
.getNumber(driveTrain
.GYRO_D_Val
,
73 driveTrain
.PID_ERROR
);
75 double setpoint
= SmartDashboard
.getNumber(driveTrain
.DRIVE_TARGET_DIST
,
76 driveTrain
.PID_ERROR
);
77 double maxTimeOut
= SmartDashboard
.getNumber(driveTrain
.MAX_TIME_OUT
,
78 driveTrain
.PID_ERROR
);
80 driveTrain
.getDriveController().setName("Drive");
81 driveTrain
.getGyroController().setName("Gyro");
83 driveTrain
.getDriveController().setConstants(driveP
, driveI
, driveD
);
84 driveTrain
.getGyroController().setConstants(gyroP
, gyroI
, gyroD
);
86 Scheduler
.getInstance()
87 .add(new DriveDistance(setpoint
, maxTimeOut
));
91 public void autonomousPeriodic() {
92 Scheduler
.getInstance().run();
97 public void teleopInit() {
102 public void teleopPeriodic() {
103 Scheduler
.getInstance().run();