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

Commit 4930d79

Browse files
dfang314dfang314
committed
changed moveControllers to 2: one for each side of the dt. NOTICE: turnController still one PIDController
1 parent 76af151 commit 4930d79

8 files changed

Lines changed: 211 additions & 64 deletions

File tree

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

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,9 @@
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;
89
import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
10+
import org.usfirst.frc.team199.Robot2018.subsystems.RightDrive;
911

1012
import edu.wpi.first.wpilibj.IterativeRobot;
1113
import edu.wpi.first.wpilibj.command.Command;
@@ -28,6 +30,8 @@ public class Robot extends IterativeRobot {
2830
public static final Lift lift = new Lift();
2931
public static RobotMap rmap;
3032
public static Drivetrain dt;
33+
public static LeftDrive ld;
34+
public static RightDrive rd;
3135
public static Listener listen;
3236

3337
public static OI oi;
@@ -57,6 +61,8 @@ public static boolean getBool(String key, boolean def) {
5761
public void robotInit() {
5862
rmap = new RobotMap();
5963
dt = new Drivetrain();
64+
ld = new LeftDrive();
65+
rd = new RightDrive();
6066
oi = new OI();
6167
listen = new Listener();
6268
// chooser.addObject("My Auto", new MyAutoCommand());

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

Lines changed: 26 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,9 @@ public class RobotMap {
4848
public static SpeedControllerGroup dtRight;
4949
public static DifferentialDrive robotDrive;
5050
public static PIDController turnController;
51-
public static PIDController moveController;
52-
// public static PIDController moveLeftController;
53-
// public static PIDController moveRightController;
51+
// public static PIDController moveController;
52+
public static PIDController moveLeftController;
53+
public static PIDController moveRightController;
5454

5555
public static AHRS ahrs;
5656
public static AnalogGyro dtGyro;
@@ -70,7 +70,7 @@ private void configSRX(WPI_TalonSRX mc) {
7070
mc.configPeakOutputForward(1, kTimeout);
7171
mc.configPeakOutputReverse(-1, kTimeout);
7272
}
73-
73+
7474
/**
7575
* This function takes in a VictorSPX motorController and sets nominal and peak
7676
* outputs to the default
@@ -110,31 +110,28 @@ public RobotMap() {
110110
turnController.setOutputRange(-1.0, 1.0);
111111
turnController.setContinuous();
112112
turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));
113-
moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
114-
Robot.getConst("MovekD", 0), Robot.dt, Robot.dt);
115-
moveController.disable();
116-
moveController.setInputRange(0, Double.MAX_VALUE);
117-
moveController.setOutputRange(-1.0, 1.0);
118-
moveController.setContinuous(false);
119-
moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
120-
// moveLeftController = new PIDController(Robot.getConst("ConstMoveLeftkP", 1),
121-
// Robot.getConst("ConstMoveLeftkI", 0), Robot.getConst("ConstMoveLeftkD", 0),
122-
// Robot.dt.getLeftDrive(), (PIDOutput) Robot.dt.getLeftDrive());
123-
// moveLeftController.disable();
124-
// moveLeftController.setInputRange(0, Double.MAX_VALUE);
125-
// moveLeftController.setOutputRange(-1.0, 1.0);
126-
// moveLeftController.setContinuous(false);
127-
// moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft",
128-
// 2));
129-
// moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP",
130-
// 1), Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD",
131-
// 0), Robot.dt.getRightDrive(), (PIDOutput) Robot.dt.getRightDrive());
132-
// moveRightController.disable();
133-
// moveRightController.setInputRange(0, Double.MAX_VALUE);
134-
// moveRightController.setOutputRange(-1.0, 1.0);
135-
// moveRightController.setContinuous(false);
136-
// moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight",
137-
// 2));
113+
// moveController = new PIDController(Robot.getConst("MovekP", 1),
114+
// Robot.getConst("MovekI", 0),
115+
// Robot.getConst("MovekD", 0), Robot.dt, Robot.dt);
116+
// moveController.disable();
117+
// moveController.setInputRange(0, Double.MAX_VALUE);
118+
// moveController.setOutputRange(-1.0, 1.0);
119+
// moveController.setContinuous(false);
120+
// moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
121+
moveLeftController = new PIDController(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0),
122+
Robot.getConst("MoveLeftkD", 0), Robot.ld, Robot.ld);
123+
moveLeftController.disable();
124+
moveLeftController.setInputRange(0, Double.MAX_VALUE);
125+
moveLeftController.setOutputRange(-1.0, 1.0);
126+
moveLeftController.setContinuous(false);
127+
moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
128+
moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1),
129+
Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.rd, Robot.rd);
130+
moveRightController.disable();
131+
moveRightController.setInputRange(0, Double.MAX_VALUE);
132+
moveRightController.setOutputRange(-1.0, 1.0);
133+
moveRightController.setContinuous(false);
134+
moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));
138135

139136
ahrs = new AHRS(SerialPort.Port.kMXP);
140137
dtGyro = new AnalogGyro(getPort("Gyro", 0));

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,12 +25,13 @@ public PIDMove(double targ, DrivetrainInterface dt) {
2525
protected void initialize() {
2626
dt.resetEnc();
2727
dt.enableMovePid();
28-
dt.setMoveSetpoint(target);
28+
dt.setMoveSetpointLeft(target);
29+
dt.setMoveSetpointRight(target);
2930
}
3031

3132
// Called repeatedly when this Command is scheduled to run
3233
protected void execute() {
33-
dt.arcadeDrive(dt.getPidOut(), 0);
34+
dt.tankDrive(dt.getLeftPidOut(), dt.getRightPidOut());
3435
}
3536

3637
// Make this return true when this Command no longer needs to run execute()

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ protected void initialize() {
3030

3131
// Called repeatedly when this Command is scheduled to run
3232
protected void execute() {
33-
dt.arcadeDrive(0, dt.getPidOut());
33+
dt.arcadeDrive(0, dt.getAnglePidOut());
3434
}
3535

3636
// Make this return true when this Command no longer needs to run execute()

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java‎

Lines changed: 89 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,11 @@
1818
import edu.wpi.first.wpilibj.Encoder;
1919
import edu.wpi.first.wpilibj.PIDController;
2020
import edu.wpi.first.wpilibj.PIDOutput;
21-
import edu.wpi.first.wpilibj.PIDSource;
22-
import edu.wpi.first.wpilibj.PIDSourceType;
2321
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2422
import edu.wpi.first.wpilibj.command.Subsystem;
2523
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
2624

27-
public class Drivetrain extends Subsystem implements PIDOutput, PIDSource, DrivetrainInterface {
25+
public class Drivetrain extends Subsystem implements PIDOutput, DrivetrainInterface {
2826
// Put methods for controlling this subsystem
2927
// here. Call these from Commands.
3028

@@ -34,24 +32,31 @@ public class Drivetrain extends Subsystem implements PIDOutput, PIDSource, Drive
3432
private final SpeedControllerGroup dtRight = RobotMap.dtRight;
3533
private final DifferentialDrive robotDrive = RobotMap.robotDrive;
3634
private final PIDController turnController = RobotMap.turnController;
37-
private final PIDController moveController = RobotMap.moveController;
35+
// private final PIDController moveController = RobotMap.moveController;
36+
private final PIDController moveLeftController = RobotMap.moveLeftController;
37+
private final PIDController moveRightController = RobotMap.moveRightController;
3838

3939
private final AHRS ahrs = RobotMap.ahrs;
4040
private final AnalogGyro dtGyro = RobotMap.dtGyro;
4141
private final DoubleSolenoid dtGear = RobotMap.dtGear;
4242

43-
private double pidOut = 0;
43+
private double anglePidOut = 0;
44+
private double leftPidOut = 0;
45+
private double rightPidOut = 0;
4446

4547
public void initDefaultCommand() {
4648
setDefaultCommand(new TeleopDrive());
4749
}
48-
50+
4951
/**
5052
* Updates the PIDControllers' PIDConstants based on SmartDashboard values
5153
*/
5254
public void updatePidConstants() {
5355
turnController.setPID(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0), Robot.getConst("TurnkD", 0));
54-
moveController.setPID(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0), Robot.getConst("MovekD", 0));
56+
moveLeftController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0),
57+
Robot.getConst("MoveLeftkD", 0));
58+
moveRightController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0),
59+
Robot.getConst("MoveRightkD", 0));
5560
}
5661

5762
/**
@@ -235,24 +240,36 @@ public void setTurnSetpoint(double set) {
235240
* Enable the movePID PIDController used for moving
236241
*/
237242
public void enableMovePid() {
238-
moveController.enable();
243+
moveLeftController.enable();
244+
moveRightController.enable();
239245
}
240246

241247
/**
242248
* Disables the movePID PIDController used for moving
243249
*/
244250
public void disableMovePid() {
245-
moveController.disable();
251+
moveLeftController.disable();
252+
moveRightController.enable();
246253
}
247254

248255
/**
249-
* Sets the setPoint of the movePID PIDController
256+
* Sets the setPoint of the moveLeftPID PIDController
250257
*
251258
* @param set
252259
* The value to set the setPoint at
253260
*/
254-
public void setMoveSetpoint(double set) {
255-
moveController.setSetpoint(set);
261+
public void setMoveSetpointLeft(double set) {
262+
moveLeftController.setSetpoint(set);
263+
}
264+
265+
/**
266+
* Sets the setPoint of the moveRightPID PIDController
267+
*
268+
* @param set
269+
* The value to set the setPoint at
270+
*/
271+
public void setMoveSetpointRight(double set) {
272+
moveRightController.setSetpoint(set);
256273
}
257274

258275
/**
@@ -280,15 +297,45 @@ public void setDistancePerPulseRight(double dist) {
280297
*
281298
* @return The value that is written by PIDControllers
282299
*/
283-
public double getPidOut() {
284-
return pidOut;
300+
public double getAnglePidOut() {
301+
return anglePidOut;
302+
}
303+
304+
/**
305+
* Returns the value that leftdrive should be set to according to PIDControllers
306+
*
307+
* @return The value that is written by PIDControllers
308+
*/
309+
public double getLeftPidOut() {
310+
return leftPidOut;
311+
}
312+
313+
/**
314+
* Returns the value that rightdrive should be set to according to PIDControllers
315+
*
316+
* @return The value that is written by PIDControllers
317+
*/
318+
public double getRightPidOut() {
319+
return rightPidOut;
285320
}
286321

287-
@Override
288-
public double pidGet() {
289-
return average(leftEnc.getDistance(), rightEnc.getDistance());
322+
/**
323+
* Returns the distance that the left encoder reads
324+
*
325+
* @return How much the left encoder has travelled
326+
*/
327+
public double getLeftEnc() {
328+
return leftEnc.getDistance();
290329
}
291330

331+
/**
332+
* Returns the distance that the right encoder reads
333+
*
334+
* @return How much the right encoder has travelled
335+
*/
336+
public double getRightEnc() {
337+
return rightEnc.getDistance();
338+
}
292339
// public boolean onTargetLeft() {
293340
// return moveLeftController.onTarget();
294341
// }
@@ -298,7 +345,27 @@ public double pidGet() {
298345

299346
@Override
300347
public void pidWrite(double output) {
301-
pidOut = output;
348+
anglePidOut = output;
349+
}
350+
351+
/**
352+
* Sets the leftPidOutput
353+
*
354+
* @param output
355+
* The value to set the output to
356+
*/
357+
public void pidLeftWrite(double output) {
358+
leftPidOut = output;
359+
}
360+
361+
/**
362+
* Sets the rightPidOutput
363+
*
364+
* @param output
365+
* The value to set the output to
366+
*/
367+
public void pidRightWrite(double output) {
368+
rightPidOut = output;
302369
}
303370

304371
/**
@@ -324,20 +391,13 @@ public boolean onTurnTarg() {
324391
}
325392

326393
/**
327-
* Returns whether the moveController PIDController senses that it's on target
394+
* Returns whether the moveLeftController and moveRightController PIDController
395+
* sense that their on target
328396
*
329-
* @return Whether the moveController PIDController is on target
397+
* @return Whether the moveController PIDControllers are on target
330398
*/
331399
public boolean onDriveTarg() {
332-
return moveController.onTarget();
400+
return moveLeftController.onTarget() && moveRightController.onTarget();
333401
}
334402

335-
@Override
336-
public void setPIDSourceType(PIDSourceType pidSource) {
337-
}
338-
339-
@Override
340-
public PIDSourceType getPIDSourceType() {
341-
return PIDSourceType.kDisplacement;
342-
}
343403
}

‎Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java‎

Lines changed: 26 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -132,12 +132,20 @@ public interface DrivetrainInterface {
132132
public void disableMovePid();
133133

134134
/**
135-
* Sets the setPoint of the movePID PIDController
135+
* Sets the setPoint of the moveLeftPID PIDController
136136
*
137137
* @param set
138138
* The value to set the setPoint at
139139
*/
140-
public void setMoveSetpoint(double set);
140+
public void setMoveSetpointLeft(double set);
141+
142+
/**
143+
* Sets the setPoint of the moveRightPID PIDController
144+
*
145+
* @param set
146+
* The value to set the setPoint at
147+
*/
148+
public void setMoveSetpointRight(double set);
141149

142150
/**
143151
* Sets the distancePerPulse property on the left encoder
@@ -160,7 +168,22 @@ public interface DrivetrainInterface {
160168
*
161169
* @return The value that is written by PIDControllers
162170
*/
163-
public double getPidOut();
171+
public double getAnglePidOut();
172+
173+
/**
174+
* Returns the value that leftdrive should be set to according to PIDControllers
175+
*
176+
* @return The value that is written by PIDControllers
177+
*/
178+
public double getLeftPidOut();
179+
180+
/**
181+
* Returns the value that rightdrive should be set to according to
182+
* PIDControllers
183+
*
184+
* @return The value that is written by PIDControllers
185+
*/
186+
public double getRightPidOut();
164187

165188
/**
166189
* Returns whether the turnController PIDController senses that it's on target

0 commit comments

Comments
 (0)