/
Robot.java
145 lines (119 loc) · 4.45 KB
/
Robot.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
package org.usfirst.frc.team4946.robot;
import org.usfirst.frc.team4946.robot.commands.autonomous.AutonomousWrapperTurningFromBack;
import org.usfirst.frc.team4946.robot.subsystems.BallIntake;
import org.usfirst.frc.team4946.robot.subsystems.DriveTrain;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends IterativeRobot {
public static BallIntake ballSubsystem;
public static DriveTrain driveSubsystem;
public static OI oi;
Command auto;
SendableChooser<Integer> m_autoMode;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
ballSubsystem = new BallIntake();
driveSubsystem = new DriveTrain();
oi = new OI();
m_autoMode = new SendableChooser<Integer>();
m_autoMode.addDefault("Left Position",
RobotConstants.Auto.LEFT_POSITION);
m_autoMode.addObject("Right Position",
RobotConstants.Auto.RIGHT_POSITION);
// m_autoMode.addObject("Middle Position Breach Left - with Shoot",
// RobotConstants.Auto.MIDDLE_POSITION_BREACH_LEFT_SHOOT);
// m_autoMode.addObject("Middle Position Breach Right - No Shoot",
// RobotConstants.Auto.MIDDLE_POSITION_BREACH_RIGHT_NO_SHOOT);
// m_autoMode.addObject("Middle Position No Breach - With Shoot",
// RobotConstants.Auto.MIDDLE_POSITION_NO_BREACH_SHOOT);
// m_autoMode.addObject("Right Position- No Shoot",
// RobotConstants.Auto.RIGHT_POSITION_NO_SHOOT);
SmartDashboard.putData("Autonomous - Script", m_autoMode);
driveSubsystem.calibrateGyroscope();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
Scheduler.getInstance().run();
}
/**
* This autonomous (along with the chooser code above) shows how to select
* between different autonomous modes using the dashboard. The sendable
* chooser code works with the Java SmartDashboard. If you prefer the
* LabVIEW Dashboard, remove all of the chooser code and uncomment the
* getString code to get the auto name from the text box below the Gyro
*
* You can add additional auto modes by adding additional commands to the
* chooser code above (like the commented example) or additional comparisons
* to the switch structure below with additional strings & commands.
*/
@Override
public void autonomousInit() {
boolean isRed = DriverStation.getInstance().getAlliance() == Alliance.Red;
int autoMode = m_autoMode.getSelected();
auto = new AutonomousWrapperTurningFromBack(autoMode, isRed);
auto.start();
}
/**
* This function is called periodically during autonomous
*/
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putNumber("Encoder Distance: ",
driveSubsystem.getEncoderDistance());
SmartDashboard.putNumber("Gyro: ", driveSubsystem.getGyroValue());
}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
driveSubsystem.resetEncoders();
driveSubsystem.resetGyro();
if (auto != null)
auto.cancel();
}
/**
* This function is called periodically during operator control
*/
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putNumber("Encoder Distance: ",
driveSubsystem.getEncoderDistance());
SmartDashboard.putNumber("Gyro: ", driveSubsystem.getGyroValue());
}
/**
* This function is called periodically during test mode
*/
@Override
public void testPeriodic() {
LiveWindow.run();
}
}