Skip to content
This repository was archived by the owner on Sep 14, 2019. It is now read-only.

Commit dbf634b

Browse files
dfang314dfang314
committed
Finished initialization of RobotMap, finished getting Drivetrain to implementing PIDSource, moved logic from TeleopDrive command to Drivetrain subsystem, and finished integrating PID in PIDTurn and PIDDrive.
1 parent e0c0cfa commit dbf634b

7 files changed

Lines changed: 262 additions & 215 deletions

File tree

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@
77

88
package org.usfirst.frc.team199.Robot2018;
99

10-
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
1110
import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
11+
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
1212

1313
import edu.wpi.first.wpilibj.Joystick;
1414
import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -46,26 +46,26 @@ public class OI {
4646
// Start the command when the button is released and let it run the command
4747
// until it is finished as determined by it's isFinished method.
4848
// button.whenReleased(new ExampleCommand());
49-
49+
5050
public Joystick leftJoy;
51-
public JoystickButton shiftDrive;
52-
public JoystickButton shiftDriveType;
51+
public JoystickButton shiftDrive;
52+
public JoystickButton shiftDriveType;
5353
public Joystick rightJoy;
5454
public Joystick manipulator;
55-
56-
55+
5756
public int getButton(String key, int def) {
58-
if(!SmartDashboard.containsKey(key)) {
57+
if (!SmartDashboard.containsKey(key)) {
5958
SmartDashboard.putNumber(key, def);
6059
}
6160
return (int) SmartDashboard.getNumber(key, def);
6261
}
62+
6363
public OI() {
6464
leftJoy = new Joystick(0);
65-
shiftDrive = new JoystickButton(leftJoy, getButton("Button Shift Drive", 1));
66-
shiftDrive.whenPressed(new ShiftLowGear());
67-
shiftDriveType = new JoystickButton(leftJoy, getButton("Button Shift Drive Type", 2));
68-
shiftDriveType.whenPressed(new ShiftDriveType());
65+
shiftDrive = new JoystickButton(leftJoy, getButton("Button Shift Drive", 1));
66+
shiftDrive.whenPressed(new ShiftLowGear());
67+
shiftDriveType = new JoystickButton(leftJoy, getButton("Button Shift Drive Type", 2));
68+
shiftDriveType.whenPressed(new ShiftDriveType());
6969
rightJoy = new Joystick(1);
7070
manipulator = new Joystick(2);
7171
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java‎

Lines changed: 31 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
import edu.wpi.first.wpilibj.IterativeRobot;
1313
import edu.wpi.first.wpilibj.command.Command;
1414
import edu.wpi.first.wpilibj.command.Scheduler;
15-
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
1615
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
1716
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1817

@@ -24,50 +23,52 @@
2423
* directory.
2524
*/
2625
public class Robot extends IterativeRobot {
27-
26+
2827
public static final Climber climber = new Climber();
2928
public static final ClimberAssist climberAssist = new ClimberAssist();
3029
public static final IntakeEject intakeEject = new IntakeEject();
3130
public static final Lift lift = new Lift();
31+
public static RobotMap rmap;
3232
public static Drivetrain dt;
3333

3434
public static OI oi;
3535

3636
Command autonomousCommand;
3737
SendableChooser<Command> chooser = new SendableChooser<>();
3838

39-
4039
public static double getConst(String key, double def) {
41-
if(!SmartDashboard.containsKey(key)) {
40+
if (!SmartDashboard.containsKey(key)) {
4241
SmartDashboard.putNumber(key, def);
4342
}
4443
return SmartDashboard.getNumber(key, def);
4544
}
45+
4646
public void sendValuesToDashboard() {
4747
ArrayList<String> boolKeys = new ArrayList<String>();
4848
ArrayList<Boolean> boolDef = new ArrayList<Boolean>();
49-
boolKeys.add("Arcade Drive");
50-
boolKeys.add("Arcade Drive Default Setup");
51-
boolKeys.add("Square Drive Values");
52-
boolKeys.add("High Gear");
53-
54-
boolDef.add(true);
55-
boolDef.add(true);
56-
boolDef.add(false);
57-
boolDef.add(false);
58-
for(int i = 0; i < boolKeys.size(); i++) {
59-
if(!SmartDashboard.containsKey(boolKeys.get(i))) {
49+
boolKeys.add("Arcade Drive");
50+
boolKeys.add("Arcade Drive Default Setup");
51+
boolKeys.add("Square Drive Values");
52+
boolKeys.add("High Gear");
53+
54+
boolDef.add(true);
55+
boolDef.add(true);
56+
boolDef.add(false);
57+
boolDef.add(false);
58+
for (int i = 0; i < boolKeys.size(); i++) {
59+
if (!SmartDashboard.containsKey(boolKeys.get(i))) {
6060
SmartDashboard.putBoolean(boolKeys.get(i), boolDef.get(i));
6161
}
6262
}
6363
}
6464

6565
/**
66-
* This function is run when the robot is first started up and should be
67-
* used for any initialization code.
66+
* This function is run when the robot is first started up and should be used
67+
* for any initialization code.
6868
*/
6969
@Override
7070
public void robotInit() {
71+
rmap = new RobotMap();
7172
dt = new Drivetrain();
7273
oi = new OI();
7374
sendValuesToDashboard();
@@ -76,9 +77,9 @@ public void robotInit() {
7677
}
7778

7879
/**
79-
* This function is called once each time the robot enters Disabled mode.
80-
* You can use it to reset any subsystem information you want to clear when
81-
* the robot is disabled.
80+
* This function is called once each time the robot enters Disabled mode. You
81+
* can use it to reset any subsystem information you want to clear when the
82+
* robot is disabled.
8283
*/
8384
@Override
8485
public void disabledInit() {
@@ -92,24 +93,24 @@ public void disabledPeriodic() {
9293

9394
/**
9495
* This autonomous (along with the chooser code above) shows how to select
95-
* between different autonomous modes using the dashboard. The sendable
96-
* chooser code works with the Java SmartDashboard. If you prefer the
97-
* LabVIEW Dashboard, remove all of the chooser code and uncomment the
98-
* getString code to get the auto name from the text box below the Gyro
96+
* between different autonomous modes using the dashboard. The sendable chooser
97+
* code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
98+
* remove all of the chooser code and uncomment the getString code to get the
99+
* auto name from the text box below the Gyro
99100
*
100101
* You can add additional auto modes by adding additional commands to the
101-
* chooser code above (like the commented example) or additional comparisons
102-
* to the switch structure below with additional strings & commands.
102+
* chooser code above (like the commented example) or additional comparisons to
103+
* the switch structure below with additional strings & commands.
103104
*/
104105
@Override
105106
public void autonomousInit() {
106107
autonomousCommand = chooser.getSelected();
107108

108109
/*
109-
* String autoSelected = SmartDashboard.getString("Auto Selector",
110-
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
111-
* = new MyAutoCommand(); break; case "Default Auto": default:
112-
* autonomousCommand = new ExampleCommand(); break; }
110+
* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
111+
* switch(autoSelected) { case "My Auto": autonomousCommand = new
112+
* MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
113+
* ExampleCommand(); break; }
113114
*/
114115

115116
// schedule the autonomous command (example)

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java‎

Lines changed: 63 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -35,91 +35,104 @@ public class RobotMap {
3535
// number and the module. For example you with a rangefinder:
3636
// public static int rangefinderPort = 1;
3737
// public static int rangefinderModule = 1;
38-
38+
3939
public static Encoder leftEnc;
4040
public static WPI_TalonSRX dtLeftDrive;
41-
public static WPI_TalonSRX dtLeftSlave;
41+
public static WPI_TalonSRX dtLeftSlave;
4242
public static SpeedControllerGroup dtLeft;
43-
43+
4444
public static Encoder rightEnc;
4545
public static WPI_TalonSRX dtRightDrive;
46-
public static WPI_TalonSRX dtRightSlave;
46+
public static WPI_TalonSRX dtRightSlave;
4747
public static SpeedControllerGroup dtRight;
4848
public static DifferentialDrive robotDrive;
4949
public static PIDController turnController;
5050
public static PIDController moveController;
51-
// public static PIDController moveLeftController;
52-
// public static PIDController moveRightController;
53-
51+
// public static PIDController moveLeftController;
52+
// public static PIDController moveRightController;
53+
5454
public static AHRS ahrs;
5555
public static AnalogGyro dtGyro;
5656
public static DoubleSolenoid dtGear;
57-
57+
5858
private void configSRX(WPI_TalonSRX mc) {
5959
int kTimeout = (int) Robot.getConst("ConstkTimeoutMs", 10);
6060
mc.configNominalOutputForward(0, kTimeout);
61-
mc.configNominalOutputReverse(0, kTimeout);
62-
mc.configPeakOutputForward(1, kTimeout);
63-
mc.configPeakOutputReverse(-1, kTimeout);
61+
mc.configNominalOutputReverse(0, kTimeout);
62+
mc.configPeakOutputForward(1, kTimeout);
63+
mc.configPeakOutputReverse(-1, kTimeout);
6464
}
65+
6566
public RobotMap() {
66-
67+
6768
leftEnc = new Encoder(getPort("Port1LeftEnc", 0), getPort("Port2LeftEnc", 1));
6869
dtLeftDrive = new WPI_TalonSRX(getPort("PortLeftTalonSRXDrive", 1));
6970
configSRX(dtLeftDrive);
70-
dtLeftSlave = new WPI_TalonSRX(getPort("PortLeftTalonSRXSlave", 1));
71-
configSRX(dtLeftSlave);
71+
dtLeftSlave = new WPI_TalonSRX(getPort("PortLeftTalonSRXSlave", 1));
72+
configSRX(dtLeftSlave);
7273
dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave);
73-
74+
7475
rightEnc = new Encoder(getPort("Port1RightEnc", 2), getPort("Port2RightEnc", 3));
7576
dtRightDrive = new WPI_TalonSRX(getPort("PortRightTalonSRXDrive", 2));
7677
configSRX(dtRightDrive);
77-
dtRightSlave = new WPI_TalonSRX(getPort("PortRightTalonSRXSlave", 1));
78-
configSRX(dtRightSlave);
78+
dtRightSlave = new WPI_TalonSRX(getPort("PortRightTalonSRXSlave", 1));
79+
configSRX(dtRightSlave);
7980
dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave);
80-
81+
8182
robotDrive = new DifferentialDrive(dtLeft, dtRight);
82-
turnController = new PIDController(Robot.getConst("ConstTurnkP", 1), Robot.getConst("ConstTurnkI", 0), Robot.getConst("ConstTurnkD", 0), ahrs, Robot.dt);
83-
turnController.disable();
84-
turnController.setInputRange(-180, 180);
85-
turnController.setOutputRange(-1.0, 1.0);
86-
turnController.setContinuous();
87-
turnController.setAbsoluteTolerance(Robot.getConst("ConstTurnTolerance", 1));
88-
moveController = new PIDController(Robot.getConst("ConstMovekP", 1), Robot.getConst("ConstMovekI", 0), Robot.getConst("ConstMovekD", 0), Robot.dt, Robot.dt);
89-
turnController.disable();
90-
turnController.setInputRange(-180, 180);
91-
turnController.setOutputRange(-1.0, 1.0);
92-
turnController.setContinuous();
93-
turnController.setAbsoluteTolerance(Robot.getConst("ConstMoveTolerance", 2));
94-
// moveLeftController = new PIDController(Robot.getConst("ConstMoveLeftkP", 1), Robot.getConst("ConstMoveLeftkI", 0), Robot.getConst("ConstMoveLeftkD", 0), Robot.dt.getLeftDrive(), (PIDOutput) Robot.dt.getLeftDrive());
95-
// moveLeftController.disable();
96-
// moveLeftController.setInputRange(0, Double.MAX_VALUE);
97-
// moveLeftController.setOutputRange(-1.0, 1.0);
98-
// moveLeftController.setContinuous(false);
99-
// moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
100-
// moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1), Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.dt.getRightDrive(), (PIDOutput) Robot.dt.getRightDrive());
101-
// moveRightController.disable();
102-
// moveRightController.setInputRange(0, Double.MAX_VALUE);
103-
// moveRightController.setOutputRange(-1.0, 1.0);
104-
// moveRightController.setContinuous(false);
105-
// moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));
106-
83+
turnController = new PIDController(Robot.getConst("ConstTurnkP", 1), Robot.getConst("ConstTurnkI", 0),
84+
Robot.getConst("ConstTurnkD", 0), ahrs, Robot.dt);
85+
turnController.disable();
86+
turnController.setInputRange(-180, 180);
87+
turnController.setOutputRange(-1.0, 1.0);
88+
turnController.setContinuous();
89+
turnController.setAbsoluteTolerance(Robot.getConst("ConstTurnTolerance", 1));
90+
moveController = new PIDController(Robot.getConst("ConstMovekP", 1), Robot.getConst("ConstMovekI", 0),
91+
Robot.getConst("ConstMovekD", 0), Robot.dt, Robot.dt);
92+
turnController.disable();
93+
turnController.setInputRange(-180, 180);
94+
turnController.setOutputRange(-1.0, 1.0);
95+
turnController.setContinuous();
96+
turnController.setAbsoluteTolerance(Robot.getConst("ConstMoveTolerance", 2));
97+
// moveLeftController = new PIDController(Robot.getConst("ConstMoveLeftkP", 1),
98+
// Robot.getConst("ConstMoveLeftkI", 0), Robot.getConst("ConstMoveLeftkD", 0),
99+
// Robot.dt.getLeftDrive(), (PIDOutput) Robot.dt.getLeftDrive());
100+
// moveLeftController.disable();
101+
// moveLeftController.setInputRange(0, Double.MAX_VALUE);
102+
// moveLeftController.setOutputRange(-1.0, 1.0);
103+
// moveLeftController.setContinuous(false);
104+
// moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft",
105+
// 2));
106+
// moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP",
107+
// 1), Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD",
108+
// 0), Robot.dt.getRightDrive(), (PIDOutput) Robot.dt.getRightDrive());
109+
// moveRightController.disable();
110+
// moveRightController.setInputRange(0, Double.MAX_VALUE);
111+
// moveRightController.setOutputRange(-1.0, 1.0);
112+
// moveRightController.setContinuous(false);
113+
// moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight",
114+
// 2));
115+
107116
ahrs = new AHRS(SerialPort.Port.kMXP);
108117
dtGyro = new AnalogGyro(getPort("PortGyro", 0));
109118
dtGear = new DoubleSolenoid(getPort("Port1dtGearSolenoid", 0), getPort("Port2dtGearSolenoid", 1));
110-
119+
111120
}
112-
121+
113122
/**
114-
* Used in RobotMap to find ports for robot components, getPort also puts numbers if they don't exist yet.
115-
* @param key The port key
116-
* @param def The default value
123+
* Used in RobotMap to find ports for robot components, getPort also puts
124+
* numbers if they don't exist yet.
125+
*
126+
* @param key
127+
* The port key
128+
* @param def
129+
* The default value
117130
* @return returns the Port
118131
*/
119132
public int getPort(String key, int def) {
120-
if(!SmartDashboard.containsKey(key)) {
133+
if (!SmartDashboard.containsKey(key)) {
121134
SmartDashboard.putNumber(key, def);
122135
}
123-
return (int)SmartDashboard.getNumber(key, def);
136+
return (int) SmartDashboard.getNumber(key, def);
124137
}
125138
}

0 commit comments

Comments
 (0)