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

Commit eebefb9

Browse files
DriverStationComputerDriverStationComputer
committed
robot is slow for some reason
1 parent c2257f6 commit eebefb9

9 files changed

Lines changed: 115 additions & 70 deletions

File tree

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

Lines changed: 22 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -99,24 +99,28 @@ public OI() {
9999
MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
100100

101101
manipulator = new Joystick(2);
102-
closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
103-
closeIntake.whenPressed(new CloseIntake());
104-
openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
105-
openIntake.whenPressed(new OpenIntake());
106-
// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
107-
// Button", 3));
108-
// raiseIntake.whenPressed(new RaiseIntake());
109-
// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
110-
// Button", 4));
111-
// lowerIntake.whenPressed(new LowerIntake());
112-
intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
113-
intake.whenPressed(new IntakeCube());
114-
outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
115-
outake.whenPressed(new OutakeCube());
116-
toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
117-
toggleLeftIntake.whenPressed(new ToggleLeftIntake());
118-
toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
119-
toggleRightIntake.whenPressed(new ToggleRightIntake());
102+
if (manipulator.getButtonCount() == 0) {
103+
System.out.println("Manipulator not plugged in!");
104+
} else {
105+
closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
106+
closeIntake.whenPressed(new CloseIntake());
107+
openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
108+
openIntake.whenPressed(new OpenIntake());
109+
// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
110+
// Button", 3));
111+
// raiseIntake.whenPressed(new RaiseIntake());
112+
// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
113+
// Button", 4));
114+
// lowerIntake.whenPressed(new LowerIntake());
115+
intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
116+
intake.whenPressed(new IntakeCube());
117+
outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
118+
outake.whenPressed(new OutakeCube());
119+
toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
120+
toggleLeftIntake.whenPressed(new ToggleLeftIntake());
121+
toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
122+
toggleRightIntake.whenPressed(new ToggleRightIntake());
123+
}
120124
}
121125

122126
// /**

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -224,6 +224,9 @@ public void teleopPeriodic() {
224224
// SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
225225
// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
226226
//
227+
228+
System.out.printf("Left: %1$f4.2; Right: %1$f4.2\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
229+
227230
SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
228231
SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
229232
SmartDashboard.putNumber("Right Current draw", rmap.pdp.getCurrent(11));

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

Lines changed: 1 addition & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@
88
package org.usfirst.frc.team199.Robot2018;
99

1010
import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
11-
import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
1211

1312
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
1413
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
@@ -23,7 +22,6 @@
2322
import edu.wpi.first.wpilibj.SPI;
2423
import edu.wpi.first.wpilibj.SpeedControllerGroup;
2524
import edu.wpi.first.wpilibj.VictorSP;
26-
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
2725
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2826

2927
/**
@@ -57,7 +55,6 @@ public class RobotMap {
5755
public static WPI_TalonSRX dtLeftMaster;
5856
public static WPI_VictorSPX dtLeftSlave;
5957
public static SpeedControllerGroup dtLeft;
60-
public static VelocityPIDController leftVelocityController;
6158

6259
public static DigitalSource rightEncPort1;
6360
public static DigitalSource rightEncPort2;
@@ -66,9 +63,7 @@ public class RobotMap {
6663
public static WPI_TalonSRX dtRightMaster;
6764
public static WPI_VictorSPX dtRightSlave;
6865
public static SpeedControllerGroup dtRight;
69-
public static VelocityPIDController rightVelocityController;
7066

71-
public static DifferentialDrive robotDrive;
7267
public static PIDSourceAverage distEncAvg;
7368

7469
public static AHRS fancyGyro;
@@ -162,20 +157,6 @@ public RobotMap() {
162157
// inverted bc gear boxes invert from input to output
163158
dtLeft.setInverted(true);
164159

165-
leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkI", 0), 0,
166-
/*
167-
* Robot.getConst("VelocityLeftkD", calcDefkD(Robot.getConst("Max Low Speed",
168-
* 84)))
169-
*/ 0, /* 1 / Robot.getConst("Max Low Speed", 84) */Robot.getConst("VelocityLeftkF",
170-
1 / Robot.getConst("Max Low Speed", 84)),
171-
leftEncRate, dtLeft);
172-
leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
173-
Robot.getConst("Max High Speed", 204));
174-
leftVelocityController.setOutputRange(-1.0, 1.0);
175-
leftVelocityController.setContinuous(false);
176-
leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
177-
SmartDashboard.putData(leftVelocityController);
178-
179160
rightEncPort1 = new DigitalInput(getPort("1RightEnc", 1));
180161
rightEncPort2 = new DigitalInput(getPort("2RightEnc", 0));
181162
rightEncDist = new Encoder(rightEncPort1, rightEncPort2);
@@ -194,23 +175,6 @@ public RobotMap() {
194175
// inverted bc gear boxes invert from input to output
195176
dtRight.setInverted(true);
196177

197-
rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkI", 0), 0,
198-
/*
199-
* Robot.getConst("VelocityRightkD", calcDefkD(Robot.getConst("Max Low Speed",
200-
* 84)))
201-
*/0, /* 1 / Robot.getConst("Max Low Speed", 84) */Robot.getConst("VelocityRightkF",
202-
1 / Robot.getConst("Max Low Speed", 84)),
203-
rightEncRate, dtRight);
204-
rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
205-
Robot.getConst("Max High Speed", 204));
206-
rightVelocityController.setOutputRange(-1.0, 1.0);
207-
rightVelocityController.setContinuous(false);
208-
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
209-
210-
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
211-
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
212-
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
213-
214178
distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
215179
fancyGyro = new AHRS(SPI.Port.kMXP);
216180
dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
@@ -304,4 +268,5 @@ private double convertNtokG(double newtons) {
304268
public double convertMtoIn(double meters) {
305269
return meters * 39.37;
306270
}
271+
307272
}

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

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -63,15 +63,13 @@ public void setConsts(double kP, double kI, double kD, double kF) {
6363
}
6464

6565
/**
66-
* Gets the current set voltage (setpoint) sent to the output
67-
* SpeedControllerGroup
66+
* Gets the current setpoint sent to the PID
6867
*
69-
* @return the current set voltage (setpoint/target/goal) sent to the output
70-
* SpeedControllerGroup
68+
* @return the current set setpoint/target/goal sent to the PID
7169
*/
7270
@Override
7371
public double get() {
74-
return out.get();
72+
return getSetpoint();
7573
}
7674

7775
/**

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,6 @@ public ShiftHighGear() {
2121
protected void initialize() {
2222
Robot.dt.shiftGears(true);
2323
SmartDashboard.putBoolean("High Gear", true);
24-
Robot.dt.resetVelocityPIDkFConsts();
24+
Robot.dt.resetAllVelocityPIDConsts();
2525
}
2626
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,6 @@ public ShiftLowGear() {
2121
protected void initialize() {
2222
Robot.dt.shiftGears(false);
2323
SmartDashboard.putBoolean("High Gear", false);
24-
Robot.dt.resetVelocityPIDkFConsts();
24+
Robot.dt.resetAllVelocityPIDConsts();
2525
}
2626
}

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

Lines changed: 73 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -41,13 +41,38 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
4141
private final PIDSourceAverage distEncAvg = RobotMap.distEncAvg;
4242
public final SpeedControllerGroup dtLeft = RobotMap.dtLeft;
4343
public final SpeedControllerGroup dtRight = RobotMap.dtRight;
44-
private final DifferentialDrive robotDrive = RobotMap.robotDrive;
45-
private final VelocityPIDController leftVelocityController = RobotMap.leftVelocityController;
46-
private final VelocityPIDController rightVelocityController = RobotMap.rightVelocityController;
44+
private DifferentialDrive robotDrive;
45+
private VelocityPIDController leftVelocityController;
46+
private VelocityPIDController rightVelocityController;
4747

4848
private final AHRS fancyGyro = RobotMap.fancyGyro;
4949
private final DoubleSolenoid dtGear = RobotMap.dtGear;
5050

51+
public Drivetrain() {
52+
// all 0s for controller construction because they all get set to right values
53+
// by resetAllVelocityPIDConsts
54+
leftVelocityController = new VelocityPIDController(0, 0, 0, 0, leftEncRate, dtLeft);
55+
rightVelocityController = new VelocityPIDController(0, 0, 0, 0, rightEncRate, dtRight);
56+
resetAllVelocityPIDConsts();
57+
58+
leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
59+
Robot.getConst("Max High Speed", 204));
60+
leftVelocityController.setOutputRange(-1.0, 1.0);
61+
leftVelocityController.setContinuous(false);
62+
leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
63+
SmartDashboard.putData(leftVelocityController);
64+
65+
rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
66+
Robot.getConst("Max High Speed", 204));
67+
rightVelocityController.setOutputRange(-1.0, 1.0);
68+
rightVelocityController.setContinuous(false);
69+
rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
70+
71+
robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
72+
robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
73+
// robotDrive = new DifferentialDrive(dtLeft, dtRight);
74+
}
75+
5176
@Override
5277
public void initDefaultCommand() {
5378
setDefaultCommand(new TeleopDrive());
@@ -94,11 +119,11 @@ public double getRightVPIDerror() {
94119
}
95120

96121
public double getLeftVPIDOutput() {
97-
return leftVelocityController.get();
122+
return dtLeft.get();
98123
}
99124

100125
public double getRightVPIDOutput() {
101-
return rightVelocityController.get();
126+
return dtRight.get();
102127
}
103128

104129
/**
@@ -377,6 +402,13 @@ public PIDSource getGyro() {
377402
return fancyGyro;
378403
}
379404

405+
@Override
406+
public void resetAllVelocityPIDConsts() {
407+
resetVelocityPIDkFConsts();
408+
resetVelocityPIDkDConsts();
409+
resetVelocityPIDkIConsts();
410+
}
411+
380412
/**
381413
* Reset the kf constants for both VelocityPIDControllers based on current DT
382414
* gearing (high or low gear).
@@ -388,13 +420,48 @@ public PIDSource getGyro() {
388420
*/
389421
@Override
390422
public double resetVelocityPIDkFConsts() {
391-
double newKF = 1 / getCurrentMaxSpeed();
423+
// double newKF = 1 / getCurrentMaxSpeed();
424+
// for this way of doing velocity PID, kF should always be 0
425+
double newKF = 0;
392426
leftVelocityController.setF(newKF);
393427
rightVelocityController.setF(newKF);
394428
SmartDashboard.putNumber("VPID kF", newKF);
395429
return newKF;
396430
}
397431

432+
/**
433+
* Reset the kI constants for both VelocityPIDControllers based on current DT
434+
* gearing (high or low gear).
435+
*/
436+
@Override
437+
public void resetVelocityPIDkIConsts() {
438+
// 0.011 was calculated manually. 84 is the low gear max speed, to which we
439+
// scale the constants.
440+
double newLeftkI = Robot.getConst("VelocityLeftkI", 0.011 * 84) / getCurrentMaxSpeed();
441+
double newRightkI = Robot.getConst("VelocityRightkI", 0.011 * 84) / getCurrentMaxSpeed();
442+
// I is P because wpilib is dumb
443+
leftVelocityController.setP(newLeftkI);
444+
rightVelocityController.setP(newRightkI);
445+
SmartDashboard.putNumber("VPID Left kI", newLeftkI);
446+
SmartDashboard.putNumber("VPID Right kI", newRightkI);
447+
}
448+
449+
/**
450+
* Reset the kD constants for both VelocityPIDControllers based on current DT
451+
* gearing (high or low gear).
452+
*/
453+
@Override
454+
public void resetVelocityPIDkDConsts() {
455+
// 0.012 was calculated manually. 84 is the low gear max speed, to which we
456+
// scale the constants.
457+
double newLeftkD = Robot.getConst("VelocityLeftkD", 0.012 * 84) / getCurrentMaxSpeed();
458+
double newRightkD = Robot.getConst("VelocityRightkD", 0.012 * 84) / getCurrentMaxSpeed();
459+
leftVelocityController.setD(newLeftkD);
460+
rightVelocityController.setD(newRightkD);
461+
SmartDashboard.putNumber("VPID Left kD", newLeftkD);
462+
SmartDashboard.putNumber("VPID Right kD", newRightkD);
463+
}
464+
398465
public double resetVPIDInputRanges() {
399466
double currentMaxSpd = getCurrentMaxSpeed();
400467
leftVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd);

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

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,12 @@ public interface DrivetrainInterface {
161161
*/
162162
public double resetVelocityPIDkFConsts();
163163

164+
public void resetVelocityPIDkIConsts();
165+
166+
public void resetVelocityPIDkDConsts();
167+
168+
public void resetAllVelocityPIDConsts();
169+
164170
/**
165171
* Gets the current max speed of the DT based on gearing (high or low gear)
166172
*
@@ -172,15 +178,17 @@ public interface DrivetrainInterface {
172178
* Put left and right velocity controllers (PID) on SmartDashboard.
173179
*/
174180
public void putVelocityControllersToDashboard();
175-
181+
176182
/**
177183
* Calculates a constant for calculating feed forward in PIDMove
184+
*
178185
* @return the constant
179186
*/
180187
public double getPIDMoveConstant();
181-
188+
182189
/**
183190
* Calculates a constant for calculating feed forward in PIDTurn
191+
*
184192
* @return the constant
185193
*/
186194
public double getPIDTurnConstant();

‎Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ void test1() {
4040
VelocityPIDController vPID = new VelocityPIDController(p, i, d, f, source, out);
4141

4242
vPID.set(20);
43-
assertEquals(vPID.get(), 20);
43+
assertEquals(20, vPID.get());
4444
}
4545

4646
@Test

0 commit comments

Comments
 (0)