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

Commit 4f6a659

Browse files
lhmcgannlhmcgann
committed
PID, Drivetrain, cleanup, other little things
implemented velocity PID controllers: class, RobotMap, DT centralized move and turn PID controllers in respective commands --> deleted them from RobotMap and their methods from Drivetrain made some more things InstantCommands tests for velocity controllers and new PIDSourceAverage class no tests for PIDMove or PIDTurn commands bc don't know how to get around need for SmartDashboard/NetworkTables deleted gyro stuff bc we don't need/use it: have the ahrs deleted LeftDrive and RightDrive classes and references in Robot general cleanup of Drivetrain and its interface
1 parent feb67e0 commit 4f6a659

18 files changed

Lines changed: 364 additions & 701 deletions

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

Lines changed: 2 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -24,33 +24,6 @@
2424
* interface to the commands and command groups that allow control of the robot.
2525
*/
2626
public class OI {
27-
//// CREATING BUTTONS
28-
// One type of button is a joystick button which is any button on a
29-
//// joystick.
30-
// You create one by telling it which joystick it's on and which button
31-
// number it is.
32-
// Joystick stick = new Joystick(port);
33-
// Button button = new JoystickButton(stick, buttonNumber);
34-
35-
// There are a few additional built in buttons you can use. Additionally,
36-
// by subclassing Button you can create custom triggers and bind those to
37-
// commands the same as any other Button.
38-
39-
//// TRIGGERING COMMANDS WITH BUTTONS
40-
// Once you have a button, it's trivial to bind it to a button in one of
41-
// three ways:
42-
43-
// Start the command when the button is pressed and let it run the command
44-
// until it is finished as determined by it's isFinished method.
45-
// button.whenPressed(new ExampleCommand());
46-
47-
// Run the command while the button is being held down and interrupt it once
48-
// the button is released.
49-
// button.whileHeld(new ExampleCommand());
50-
51-
// Start the command when the button is released and let it run the command
52-
// until it is finished as determined by it's isFinished method.
53-
// button.whenReleased(new ExampleCommand());
5427

5528
public Joystick leftJoy;
5629
private JoystickButton shiftLowGear;
@@ -79,9 +52,9 @@ public OI() {
7952
shiftDriveType = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
8053
shiftDriveType.whenPressed(new ShiftDriveType());
8154
PIDMove = new JoystickButton(leftJoy, getButton("PID Move", 7));
82-
PIDMove.whenPressed(new PIDMove(40, Robot.dt));
55+
PIDMove.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg));
8356
PIDTurn = new JoystickButton(leftJoy, getButton("PID Turn", 8));
84-
PIDTurn.whenPressed(new PIDTurn(30, Robot.dt));
57+
PIDTurn.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro));
8558

8659
rightJoy = new Joystick(1);
8760
updatePidConstants = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));

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

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,7 @@
55
import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist;
66
import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
77
import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
8-
import org.usfirst.frc.team199.Robot2018.subsystems.LeftDrive;
98
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
10-
import org.usfirst.frc.team199.Robot2018.subsystems.RightDrive;
119

1210
import edu.wpi.first.wpilibj.TimedRobot;
1311
import edu.wpi.first.wpilibj.command.Command;
@@ -30,8 +28,6 @@ public class Robot extends TimedRobot {
3028
public static Lift lift;
3129
public static RobotMap rmap;
3230
public static Drivetrain dt;
33-
public static LeftDrive ld;
34-
public static RightDrive rd;
3531
public static Listener listen;
3632

3733
public static OI oi;
@@ -65,9 +61,6 @@ public void robotInit() {
6561
intakeEject = new IntakeEject();
6662
lift = new Lift();
6763
dt = new Drivetrain();
68-
rmap.initPIDControllers();
69-
ld = new LeftDrive();
70-
rd = new RightDrive();
7164
oi = new OI();
7265
listen = new Listener();
7366
// chooser.addObject("My Auto", new MyAutoCommand());

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

Lines changed: 51 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -7,14 +7,19 @@
77

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

10+
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
11+
import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
12+
1013
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
1114
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
1215
import com.kauailabs.navx.frc.AHRS;
1316

1417
import edu.wpi.first.wpilibj.AnalogGyro;
18+
import edu.wpi.first.wpilibj.DigitalInput;
19+
import edu.wpi.first.wpilibj.DigitalSource;
1520
import edu.wpi.first.wpilibj.DoubleSolenoid;
1621
import edu.wpi.first.wpilibj.Encoder;
17-
import edu.wpi.first.wpilibj.PIDController;
22+
import edu.wpi.first.wpilibj.PIDSourceType;
1823
import edu.wpi.first.wpilibj.SerialPort;
1924
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2025
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -27,32 +32,28 @@
2732
* floating around.
2833
*/
2934
public class RobotMap {
30-
// For example to map the left and right motors, you could define the
31-
// following variables to use with your drivetrain subsystem.
32-
// public static int leftMotor = 1;
33-
// public static int rightMotor = 2;
34-
35-
// If you are using multiple modules, make sure to define both the port
36-
// number and the module. For example you with a rangefinder:
37-
// public static int rangefinderPort = 1;
38-
// public static int rangefinderModule = 1;
39-
40-
public static Encoder leftEnc;
35+
public static DigitalSource leftEncPort1;
36+
public static DigitalSource leftEncPort2;
37+
public static Encoder leftEncDist;
38+
public static Encoder leftEncRate;
4139
public static WPI_TalonSRX dtLeftDrive;
4240
public static WPI_VictorSPX dtLeftSlave;
4341
public static SpeedControllerGroup dtLeft;
42+
public static VelocityPIDController leftVelocityController;
4443

45-
public static Encoder rightEnc;
44+
public static DigitalSource rightEncPort1;
45+
public static DigitalSource rightEncPort2;
46+
public static Encoder rightEncDist;
47+
public static Encoder rightEncRate;
4648
public static WPI_TalonSRX dtRightDrive;
4749
public static WPI_VictorSPX dtRightSlave;
4850
public static SpeedControllerGroup dtRight;
51+
public static VelocityPIDController rightVelocityController;
52+
4953
public static DifferentialDrive robotDrive;
50-
public static PIDController turnController;
51-
// public static PIDController moveController;
52-
public static PIDController moveLeftController;
53-
public static PIDController moveRightController;
54+
public static PIDSourceAverage distEncAvg;
5455

55-
public static AHRS ahrs;
56+
public static AHRS fancyGyro;
5657
public static AnalogGyro dtGyro;
5758
public static DoubleSolenoid dtGear;
5859

@@ -88,61 +89,52 @@ private void configSPX(WPI_VictorSPX mc) {
8889

8990
public RobotMap() {
9091

91-
leftEnc = new Encoder(getPort("1LeftEnc", 0), getPort("2LeftEnc", 1));
92+
leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 0));
93+
leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 1));
94+
leftEncDist = new Encoder(leftEncPort1, leftEncPort2);
95+
leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
96+
leftEncRate = new Encoder(leftEncPort1, leftEncPort2);
97+
leftEncRate.setPIDSourceType(PIDSourceType.kRate);
9298
dtLeftDrive = new WPI_TalonSRX(getPort("LeftTalonSRXDrive", 0));
9399
configSRX(dtLeftDrive);
94100
dtLeftSlave = new WPI_VictorSPX(getPort("LeftTalonSPXSlave", 1));
95101
configSPX(dtLeftSlave);
96102
dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave);
97103

98-
rightEnc = new Encoder(getPort("1RightEnc", 2), getPort("2RightEnc", 3));
104+
leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1),
105+
Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), leftEncRate, dtLeft);
106+
leftVelocityController.enable();
107+
leftVelocityController.setInputRange(0, Double.MAX_VALUE);
108+
leftVelocityController.setOutputRange(-1.0, 1.0);
109+
leftVelocityController.setContinuous(false);
110+
leftVelocityController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
111+
112+
rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2));
113+
rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3));
114+
rightEncDist = new Encoder(leftEncPort1, leftEncPort2);
115+
rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
116+
rightEncRate = new Encoder(leftEncPort1, leftEncPort2);
117+
rightEncRate.setPIDSourceType(PIDSourceType.kRate);
99118
dtRightDrive = new WPI_TalonSRX(getPort("RightTalonSRXDrive", 2));
100119
configSRX(dtRightDrive);
101120
dtRightSlave = new WPI_VictorSPX(getPort("RightTalonSPXSlave", 3));
102121
configSPX(dtRightSlave);
103122
dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave);
104123

105-
robotDrive = new DifferentialDrive(dtLeft, dtRight);
106-
107-
// moveController = new PIDController(Robot.getConst("MovekP", 1),
108-
// Robot.getConst("MovekI", 0),
109-
// Robot.getConst("MovekD", 0), Robot.dt, Robot.dt);
110-
// moveController.disable();
111-
// moveController.setInputRange(0, Double.MAX_VALUE);
112-
// moveController.setOutputRange(-1.0, 1.0);
113-
// moveController.setContinuous(false);
114-
// moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
115-
116-
117-
ahrs = new AHRS(SerialPort.Port.kMXP);
124+
rightVelocityController = new VelocityPIDController(Robot.getConst("ConstMoveRightkP", 1),
125+
Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), rightEncRate, dtRight);
126+
rightVelocityController.enable();
127+
rightVelocityController.setInputRange(0, Double.MAX_VALUE);
128+
rightVelocityController.setOutputRange(-1.0, 1.0);
129+
rightVelocityController.setContinuous(false);
130+
rightVelocityController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));
131+
132+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
133+
134+
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
135+
fancyGyro = new AHRS(SerialPort.Port.kMXP);
118136
dtGyro = new AnalogGyro(getPort("Gyro", 0));
119137
dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
120-
121-
}
122-
123-
public void initPIDControllers() {
124-
turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),
125-
Robot.getConst("TurnkD", 0), ahrs, Robot.dt);
126-
turnController.disable();
127-
turnController.setInputRange(-180, 180);
128-
turnController.setOutputRange(-1.0, 1.0);
129-
turnController.setContinuous();
130-
turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));
131-
132-
moveLeftController = new PIDController(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0),
133-
Robot.getConst("MoveLeftkD", 0), Robot.ld, Robot.ld);
134-
moveLeftController.disable();
135-
moveLeftController.setInputRange(0, Double.MAX_VALUE);
136-
moveLeftController.setOutputRange(-1.0, 1.0);
137-
moveLeftController.setContinuous(false);
138-
moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
139-
moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1),
140-
Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.rd, Robot.rd);
141-
moveRightController.disable();
142-
moveRightController.setInputRange(0, Double.MAX_VALUE);
143-
moveRightController.setOutputRange(-1.0, 1.0);
144-
moveRightController.setContinuous(false);
145-
moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));
146138
}
147139

148140
/**

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java‎

Lines changed: 43 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -3,56 +3,88 @@
33
import edu.wpi.first.wpilibj.PIDController;
44
import edu.wpi.first.wpilibj.PIDSource;
55
import edu.wpi.first.wpilibj.SpeedController;
6+
import edu.wpi.first.wpilibj.SpeedControllerGroup;
67

78
public class VelocityPIDController extends PIDController implements SpeedController {
89

9-
private SpeedController out;
10+
private SpeedControllerGroup out;
1011

1112
/**
12-
* @param kp the proportional PID constant
13-
* @param ki the integral PID constant
14-
* @param kd the derivative PID constant
13+
* Constructs a VelocityPIDContoller and invokes the super constructor
14+
* (PIDController), setting the three PID constants and the source and output
15+
* for this VelocityPIDController.
16+
*
17+
* @param kp
18+
* the proportional PID constant
19+
* @param ki
20+
* the integral PID constant
21+
* @param kd
22+
* the derivative PID constant
1523
* @param source
16-
* the SpeedController you are reading from
24+
* the sensor (e.g. velocity encoder) you are reading from
1725
* @param output
1826
* the SpeedController you are telling what to do
1927
*/
20-
public VelocityPIDController(double kp, double ki, double kd, PIDSource source, SpeedController output) {
28+
public VelocityPIDController(double kp, double ki, double kd, PIDSource source, SpeedControllerGroup output) {
2129
super(kp, ki, kd, source, output);
2230
out = output;
2331
}
2432

33+
/**
34+
* Sets the target speed
35+
*
36+
* @param speed
37+
* the target speed [-1, 1]
38+
*/
2539
@Override
2640
public void pidWrite(double output) {
2741
setSetpoint(output);
2842
}
2943

44+
/**
45+
* Sets the target speed
46+
*
47+
* @param speed
48+
* the target speed [-1, 1]
49+
*/
3050
@Override
3151
public void set(double speed) {
32-
// TODO Auto-generated method stub
3352
setSetpoint(speed);
3453
}
3554

55+
/**
56+
* Gets the current set voltage (setpoint) sent to the output
57+
* SpeedControllerGroup
58+
*
59+
* @return the current set voltage (setpoint/target/goal) sent to the output
60+
* SpeedControllerGroup
61+
*/
3662
@Override
3763
public double get() {
38-
// TODO Auto-generated method stub
39-
// should possibly be actual spdCtr value instead???
40-
return out.get();
64+
return getSetpoint();
4165
}
4266

67+
/**
68+
*
69+
* */
4370
@Override
4471
public void setInverted(boolean isInverted) {
4572
out.setInverted(isInverted);
4673
}
4774

75+
/**
76+
*
77+
* */
4878
@Override
4979
public boolean getInverted() {
5080
return out.getInverted();
5181
}
5282

83+
/**
84+
*
85+
* */
5386
@Override
5487
public void stopMotor() {
55-
// TODO Auto-generated method stub
5688
out.stopMotor();
5789
}
5890
}

0 commit comments

Comments
 (0)