Skip to content

Commit 3dcdfec

Browse files
nootsquarednootsquared
committed
amp bar tu
1 parent e8551a8 commit 3dcdfec

6 files changed

Lines changed: 48 additions & 28 deletions

File tree

‎src/main/java/frc/robot/BuildConstants.java‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,14 +3,14 @@
33
/** Automatically generated file containing build version information. */
44
public final class BuildConstants {
55
public static final String MAVEN_GROUP = "";
6-
public static final String MAVEN_NAME = "2024 Robot Code";
6+
public static final String MAVEN_NAME = "2024RobotCode";
77
public static final String VERSION = "unspecified";
8-
public static final int GIT_REVISION = 316;
9-
public static final String GIT_SHA = "4f43aca5f7e418f2bb0dc5809baa314817c0df7a";
10-
public static final String GIT_DATE = "2024-08-10 11:20:01 EDT";
8+
public static final int GIT_REVISION = 318;
9+
public static final String GIT_SHA = "e8551a83b11b902aaddd7d48b4ea2b02323d1b44";
10+
public static final String GIT_DATE = "2024-08-10 14:29:45 EDT";
1111
public static final String GIT_BRANCH = "amp-bar";
12-
public static final String BUILD_DATE = "2024-08-10 13:30:10 EDT";
13-
public static final long BUILD_UNIX_TIME = 1723311010234L;
12+
public static final String BUILD_DATE = "2024-08-10 16:39:27 EDT";
13+
public static final long BUILD_UNIX_TIME = 1723322367710L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ public static class ElevatorConstants {
111111
public static final double[] PID = {0, 0, 0};
112112

113113
public static final double REDUCTION = (25.0 / 1.0);
114-
public static final int BAR_THRESHOLD = 0;
114+
public static final double BAR_THRESHOLD = 3;
115115
}
116116

117117
public static class PivotConstants {

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -825,6 +825,8 @@ private void driverControls() {
825825
driveAButton.onTrue(climbCommands);
826826

827827
driveXButton.onTrue(trapCommands);
828+
driveController.b().onTrue(new SetAmpBarTarget(90, 3, elevator));
829+
driveController.b().onFalse(new SetAmpBarTarget(0, 3, elevator));
828830

829831
// driveController
830832
// .rightBumper()

‎src/main/java/frc/robot/commands/SetAmpBarTarget.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66

77
import edu.wpi.first.wpilibj2.command.Command;
88
import frc.robot.subsystems.elevator.Elevator;
9+
import org.littletonrobotics.junction.Logger;
910

1011
public class SetAmpBarTarget extends Command {
1112
/** Creates a new SetAmpBarTarget. */
@@ -16,7 +17,7 @@ public class SetAmpBarTarget extends Command {
1617

1718
public SetAmpBarTarget(double setpoint, double threshold, Elevator elevator) {
1819
// Use addRequirements() here to declare subsystem dependencies.
19-
this.setPoint = setPoint;
20+
this.setPoint = setpoint;
2021
this.threshold = threshold;
2122
this.elevator = elevator;
2223

@@ -42,6 +43,7 @@ public void end(boolean interrupted) {
4243
// Returns true when the command should end.
4344
@Override
4445
public boolean isFinished() {
45-
return elevator.ampBarAtSetpoint();
46+
Logger.recordOutput("bar command ", elevator.ampBarAtGoal());
47+
return elevator.ampBarAtGoal();
4648
}
4749
}

‎src/main/java/frc/robot/subsystems/Elevator/Elevator.java‎

Lines changed: 22 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
package frc.robot.subsystems.elevator;
22

3-
import edu.wpi.first.math.MathUtil;
43
import edu.wpi.first.math.controller.ArmFeedforward;
54
import edu.wpi.first.math.controller.ElevatorFeedforward;
65
import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -48,7 +47,7 @@ public class Elevator extends SubsystemBase {
4847
private TrapezoidProfile.State barCurrent = new TrapezoidProfile.State();
4948

5049
private double goal;
51-
private double barGoalPos;
50+
// private double barGoalPos;
5251
private final ElevatorFeedforward elevatorFFModel;
5352
private final ArmFeedforward barFFmodel;
5453

@@ -66,8 +65,8 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
6665
kP.initDefault(0.44);
6766
kI.initDefault(0);
6867

69-
barkP.initDefault(0);
70-
barkV.initDefault(0);
68+
barkP.initDefault(0.04);
69+
barkV.initDefault(4);
7170
barkG.initDefault(0);
7271
break;
7372
case REPLAY:
@@ -115,8 +114,8 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
115114
extenderProfile = new TrapezoidProfile(extenderConstraints);
116115
extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
117116

118-
maxVelocityDegPerSec = 15;
119-
maxAccelerationDegPerSecSquared = 1;
117+
maxVelocityDegPerSec = 90;
118+
maxAccelerationDegPerSecSquared = 30;
120119

121120
barConstraints =
122121
new TrapezoidProfile.Constraints(maxVelocityDegPerSec, maxAccelerationDegPerSecSquared);
@@ -126,7 +125,7 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
126125
this.elevator.configurePID(kP.get(), 0, 0);
127126
elevatorFFModel = new ElevatorFeedforward(kS.get(), kG.get(), kV.get(), kA.get());
128127

129-
ampBar.configurePID(barkP.get(), 0, 0);
128+
this.ampBar.configurePID(barkP.get(), 0, 0);
130129
barFFmodel = new ArmFeedforward(0, barkG.get(), barkV.get(), 0);
131130
}
132131

@@ -135,7 +134,7 @@ public boolean atGoal() {
135134
}
136135

137136
public boolean barAtGoal() {
138-
return (Math.abs(aInputs.barPositionDegrees - barGoalPos)
137+
return (Math.abs(aInputs.barPositionDegrees - barGoal.position)
139138
<= Constants.ElevatorConstants.BAR_THRESHOLD);
140139
}
141140

@@ -152,13 +151,17 @@ private double getBarError() {
152151
return aInputs.barPositionSetpointDegrees - aInputs.barPositionDegrees;
153152
}
154153

154+
private double getbarErrorToGoal() {
155+
return barGoal.position - aInputs.barPositionDegrees;
156+
}
157+
155158
public boolean elevatorAtSetpoint() {
156159
return (Math.abs(getElevatorError()) <= Constants.ElevatorConstants.THRESHOLD);
157160
}
158161

159-
public boolean ampBarAtSetpoint() {
162+
public boolean ampBarAtGoal() {
160163

161-
return (Math.abs(getBarError()) <= Constants.ElevatorConstants.BAR_THRESHOLD);
164+
return (Math.abs(getbarErrorToGoal()) <= Constants.ElevatorConstants.BAR_THRESHOLD);
162165
}
163166

164167
public void setBarBrakeMode(boolean bool) {
@@ -171,7 +174,7 @@ public double getBarPositionRotations() {
171174

172175
public void setBarPosition(double positionRotations, double velocityDegsPerSec) {
173176

174-
positionRotations = MathUtil.clamp(positionRotations, 0, 20);
177+
// positionRotations = MathUtil.clamp(positionRotations, 0, 20);
175178
ampBar.setPositionSetpoint(
176179
positionRotations,
177180
barFFmodel.calculate(
@@ -183,8 +186,9 @@ public void stopAmpBar() {
183186
}
184187

185188
public void setBarGoal(double setpoint) {
186-
this.barGoalPos = setpoint;
189+
187190
barGoal = new TrapezoidProfile.State(setpoint, 0);
191+
Logger.recordOutput("bar goal", setpoint);
188192
}
189193

190194
public void setbarCurrent(double current) {
@@ -242,10 +246,15 @@ public void periodic() {
242246
Logger.processInputs("Amp bar inputs", aInputs);
243247

244248
Logger.recordOutput("amp bar error", getBarError());
245-
Logger.recordOutput("amp bar goal", barGoalPos);
249+
Logger.recordOutput("amp bar goal", barGoal.position);
246250

247251
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode())) {
248252
elevator.configurePID(kP.get(), kI.get(), 0);
249253
}
254+
if (barkP.hasChanged(hashCode())
255+
|| barkV.hasChanged(hashCode())
256+
|| barkG.hasChanged(hashCode())) {
257+
ampBar.configurePID(barkP.get(), 0, 0);
258+
}
250259
}
251260
}

‎src/main/java/frc/robot/subsystems/elevator/AmpBarIOSparkMAX.java‎

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ public class AmpBarIOSparkMAX implements AmpBarIO {
1212
private final CANSparkMax barMotor;
1313
private final SparkPIDController pid;
1414
private double barPositionSetpoint;
15+
private final double gearRatio = 15 / 1;
1516

1617
public AmpBarIOSparkMAX(int motorID) {
1718

@@ -22,7 +23,9 @@ public AmpBarIOSparkMAX(int motorID) {
2223

2324
barMotor.restoreFactoryDefaults();
2425
barMotor.setCANTimeout(250);
25-
barMotor.setSmartCurrentLimit(20);
26+
barMotor.setSmartCurrentLimit(40);
27+
barMotor.getEncoder().setPosition(0);
28+
2629
barMotor.burnFlash();
2730
barMotor.clearFaults();
2831

@@ -31,8 +34,8 @@ public AmpBarIOSparkMAX(int motorID) {
3134

3235
@Override
3336
public void updateInputs(AmpBarIOInputs inputs) {
34-
inputs.barVelocityDegsPerSec = (barMotor.getEncoder().getVelocity() / 15) * 6;
35-
inputs.barPositionDegrees = (barMotor.getEncoder().getPosition() / 15) * 360;
37+
inputs.barVelocityDegsPerSec = (barMotor.getEncoder().getVelocity() / gearRatio) * 360. / 60.;
38+
inputs.barPositionDegrees = (barMotor.getEncoder().getPosition() / gearRatio) * 360.;
3639
inputs.barPositionSetpointDegrees = barPositionSetpoint;
3740
inputs.currentAmps = barMotor.getOutputCurrent();
3841
inputs.appliedVolts = barMotor.getAppliedOutput();
@@ -49,11 +52,15 @@ public void setBrakeMode(boolean bool) {
4952
}
5053

5154
@Override
52-
public void setPositionSetpoint(double positionDegs, double ffVolts) {
55+
public void setPositionSetpoint(double barPositionOutputDegs, double ffVolts) {
5356

54-
this.barPositionSetpoint = positionDegs;
57+
this.barPositionSetpoint = barPositionOutputDegs;
5558
pid.setReference(
56-
(positionDegs / 360) * 15, ControlType.kPosition, 0, ffVolts, ArbFFUnits.kVoltage);
59+
(barPositionOutputDegs / 360.) * gearRatio,
60+
ControlType.kPosition,
61+
0,
62+
ffVolts,
63+
ArbFFUnits.kVoltage);
5764
}
5865

5966
@Override

0 commit comments

Comments
 (0)