Skip to content

Commit 4f43aca

Browse files
Vignesh1234587Vignesh1234587
committed
added amp bar methods in elevator and sim
1 parent 2d26a56 commit 4f43aca

8 files changed

Lines changed: 162 additions & 20 deletions

File tree

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

Lines changed: 7 additions & 7 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 = "2024RobotCode";
6+
public static final String MAVEN_NAME = "2024 Robot Code";
77
public static final String VERSION = "unspecified";
8-
public static final int GIT_REVISION = 316;
9-
public static final String GIT_SHA = "5cc22806712e7f8851e100516ff524db3c14f0e2";
10-
public static final String GIT_DATE = "2024-08-03 20:37:50 EDT";
11-
public static final String GIT_BRANCH = "rotation-lock";
12-
public static final String BUILD_DATE = "2024-08-06 15:04:45 EDT";
13-
public static final long BUILD_UNIX_TIME = 1722971085276L;
8+
public static final int GIT_REVISION = 315;
9+
public static final String GIT_SHA = "2d26a56769a1e28bbfa8f10fcdfec1139f05c5d9";
10+
public static final String GIT_DATE = "2024-08-06 15:08:41 EDT";
11+
public static final String GIT_BRANCH = "amp-bar";
12+
public static final String BUILD_DATE = "2024-08-10 11:18:25 EDT";
13+
public static final long BUILD_UNIX_TIME = 1723303105924L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -111,6 +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;
114115
}
115116

116117
public static class PivotConstants {

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

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,9 @@
6767
import frc.robot.subsystems.drive.ModuleIOTalonFX;
6868
import frc.robot.subsystems.drive.VisionIO;
6969
import frc.robot.subsystems.drive.VisionIOLimelight;
70+
import frc.robot.subsystems.elevator.AmpBarIO;
71+
import frc.robot.subsystems.elevator.AmpBarIOSIm;
72+
import frc.robot.subsystems.elevator.AmpBarIOSparkMAX;
7073
import frc.robot.subsystems.elevator.Elevator;
7174
import frc.robot.subsystems.elevator.ElevatorIO;
7275
import frc.robot.subsystems.elevator.ElevatorIOSim;
@@ -133,6 +136,7 @@ public class RobotContainer {
133136
private Trigger driveRightTrigger;
134137
private Trigger driveAButton;
135138
private Trigger driveXButton;
139+
private Trigger driveBButton;
136140

137141
private final LoggedDashboardNumber flywheelSpeed = new LoggedDashboardNumber("fly soeed", 5400);
138142

@@ -178,7 +182,8 @@ public RobotContainer() {
178182
new LeafBlowerIOTalonSRX(18));
179183
elevator =
180184
new Elevator(
181-
new ElevatorIOTalonFX(RobotMap.ElevatorIDs.LEFT, RobotMap.ElevatorIDs.RIGHT));
185+
new ElevatorIOTalonFX(RobotMap.ElevatorIDs.LEFT, RobotMap.ElevatorIDs.RIGHT),
186+
new AmpBarIOSparkMAX(RobotMap.ElevatorIDs.BAR));
182187
pivot =
183188
new Pivot(
184189
new PivotIOTalonFX(
@@ -201,7 +206,7 @@ public RobotContainer() {
201206
new FeederIOSim(),
202207
new DistanceSensorIO() {},
203208
new LeafBlowerIO() {});
204-
elevator = new Elevator(new ElevatorIOSim());
209+
elevator = new Elevator(new ElevatorIOSim(), null);
205210
pivot = new Pivot(new PivotIOSim());
206211
led = new LED(new LED_IOSim());
207212
break;
@@ -221,7 +226,7 @@ public RobotContainer() {
221226
new FeederIOSim(),
222227
new DistanceSensorIO() {},
223228
new LeafBlowerIO() {});
224-
elevator = new Elevator(new ElevatorIOSim());
229+
elevator = new Elevator(new ElevatorIOSim(), new AmpBarIOSIm());
225230
pivot = new Pivot(new PivotIOSim());
226231
led = new LED(new LED_IOSim());
227232
break;
@@ -244,7 +249,7 @@ public RobotContainer() {
244249
new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER),
245250
new DistanceSensorIO() {},
246251
new LeafBlowerIO() {});
247-
elevator = new Elevator(new ElevatorIO() {});
252+
elevator = new Elevator(new ElevatorIO() {}, new AmpBarIO() {});
248253
pivot = new Pivot(new PivotIO() {});
249254
led = new LED(new LED_IO() {});
250255
break;
@@ -267,6 +272,7 @@ public RobotContainer() {
267272
driveRightTrigger = driveController.rightTrigger();
268273
driveAButton = driveController.a();
269274
driveXButton = driveController.x();
275+
driveBButton = driveController.b();
270276

271277
// intakeLEDCommands =
272278
// new SelectCommand<>(
@@ -702,6 +708,9 @@ private void testControls() {
702708
// driveController.leftTrigger().onTrue(new InstantCommand(() -> shooter.setFeedersRPM(200)));
703709
driveController.leftTrigger().onTrue(new InstantCommand(() -> shooter.setFeedersRPM(500)));
704710
driveController.leftTrigger().onFalse(new InstantCommand(() -> shooter.stopFeeders()));
711+
712+
driveController.b().onTrue(new InstantCommand(() -> elevator.setBarGoal(1000), elevator));
713+
driveController.b().onFalse(new InstantCommand(() -> elevator.setBarGoal(0), elevator));
705714
}
706715

707716
// TODO:: change drive controls to match changed test controls

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ public static class ElevatorIDs {
1616
// public static final int RIGHT = 9;
1717
public static final int LEFT = 8;
1818
public static final int RIGHT = 9;
19+
public static final int BAR = 0;
1920
}
2021

2122
public static class PivotIDs {

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

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

3+
import edu.wpi.first.math.MathUtil;
4+
import edu.wpi.first.math.controller.ArmFeedforward;
35
import edu.wpi.first.math.controller.ElevatorFeedforward;
46
import edu.wpi.first.math.trajectory.TrapezoidProfile;
57
import edu.wpi.first.wpilibj.DriverStation;
@@ -11,8 +13,10 @@
1113
public class Elevator extends SubsystemBase {
1214

1315
private final ElevatorIO elevator;
16+
private final AmpBarIO ampBar;
1417

1518
private final ElevatorIOInputsAutoLogged eInputs = new ElevatorIOInputsAutoLogged();
19+
private final AmpBarIOInputsAutoLogged aInputs = new AmpBarIOInputsAutoLogged();
1620

1721
private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP");
1822
private static final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/kI");
@@ -22,18 +26,35 @@ public class Elevator extends SubsystemBase {
2226
private static final LoggedTunableNumber kV = new LoggedTunableNumber("Elevator/kV");
2327
private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/kA");
2428

29+
// amp bar gains
30+
31+
private static double barkP;
32+
private static double barkV;
33+
private static double barkG;
34+
2535
private TrapezoidProfile extenderProfile;
2636
private TrapezoidProfile.Constraints extenderConstraints =
2737
new TrapezoidProfile.Constraints(30, 85);
2838
private TrapezoidProfile.State extenderGoal = new TrapezoidProfile.State();
2939
private TrapezoidProfile.State extenderCurrent = new TrapezoidProfile.State();
3040

31-
private double goal;
41+
private static double maxVelocityRPM;
42+
private static double maxAccelerationRPM;
43+
44+
private TrapezoidProfile barProfile;
45+
private TrapezoidProfile.Constraints barConstraints;
46+
47+
private TrapezoidProfile.State barGoal = new TrapezoidProfile.State();
48+
private TrapezoidProfile.State barCurrent = new TrapezoidProfile.State();
3249

50+
private double goal;
51+
private double barGoalPos;
3352
private final ElevatorFeedforward elevatorFFModel;
53+
private final ArmFeedforward barFFmodel;
3454

35-
public Elevator(ElevatorIO elevator) {
55+
public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
3656
this.elevator = elevator;
57+
this.ampBar = ampBar;
3758

3859
switch (Constants.getMode()) {
3960
case REAL:
@@ -44,6 +65,10 @@ public Elevator(ElevatorIO elevator) {
4465

4566
kP.initDefault(0.44);
4667
kI.initDefault(0);
68+
69+
barkP = 0.0;
70+
barkV = 0.0;
71+
barkG = 0.0;
4772
break;
4873
case REPLAY:
4974
kS.initDefault(0);
@@ -53,6 +78,10 @@ public Elevator(ElevatorIO elevator) {
5378

5479
kP.initDefault(15);
5580
kI.initDefault(0);
81+
82+
barkP = 0.0;
83+
barkV = 0.0;
84+
barkG = 0.0;
5685
break;
5786
case SIM:
5887
kS.initDefault(0);
@@ -62,6 +91,10 @@ public Elevator(ElevatorIO elevator) {
6291

6392
kP.initDefault(1);
6493
kI.initDefault(0);
94+
95+
barkP = 0.0;
96+
barkV = 0.0;
97+
barkG = 0.0;
6598
break;
6699
default:
67100
kS.initDefault(0);
@@ -71,21 +104,40 @@ public Elevator(ElevatorIO elevator) {
71104

72105
kP.initDefault(15);
73106
kI.initDefault(0);
107+
108+
barkP = 0.0;
109+
barkV = 0.0;
110+
barkG = 0.0;
74111
break;
75112
}
76113

77114
setExtenderGoal(0.162);
78115
extenderProfile = new TrapezoidProfile(extenderConstraints);
79116
extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
80117

118+
maxVelocityRPM = 15;
119+
maxAccelerationRPM = 1;
120+
121+
barConstraints = new TrapezoidProfile.Constraints(maxVelocityRPM, maxAccelerationRPM);
122+
barProfile = new TrapezoidProfile(barConstraints);
123+
barCurrent = barProfile.calculate(0, barCurrent, barGoal);
124+
81125
this.elevator.configurePID(kP.get(), 0, 0);
82126
elevatorFFModel = new ElevatorFeedforward(kS.get(), kG.get(), kV.get(), kA.get());
127+
128+
ampBar.configurePID(barkP, 0, 0);
129+
barFFmodel = new ArmFeedforward(0, barkG, barkV, 0);
83130
}
84131

85132
public boolean atGoal() {
86133
return (Math.abs(eInputs.elevatorPosition - goal) <= Constants.ElevatorConstants.THRESHOLD);
87134
}
88135

136+
public boolean barAtGoal() {
137+
return (Math.abs(aInputs.barPositionRotations - barGoalPos)
138+
<= Constants.ElevatorConstants.BAR_THRESHOLD);
139+
}
140+
89141
public double getElevatorPosition() {
90142
return eInputs.elevatorPosition;
91143
}
@@ -94,10 +146,50 @@ private double getElevatorError() {
94146
return eInputs.positionSetpoint - eInputs.elevatorPosition;
95147
}
96148

149+
private double getBarError() {
150+
151+
return aInputs.barPositionSetpointRotations - aInputs.barPositionRotations;
152+
}
153+
97154
public boolean elevatorAtSetpoint() {
98155
return (Math.abs(getElevatorError()) <= Constants.ElevatorConstants.THRESHOLD);
99156
}
100157

158+
public boolean ampBarAtSetpoint() {
159+
160+
return (Math.abs(getBarError()) <= Constants.ElevatorConstants.BAR_THRESHOLD);
161+
}
162+
163+
public void setBarBrakeMode(boolean bool) {
164+
ampBar.setBrakeMode(bool);
165+
}
166+
167+
public double getBarPositionRotations() {
168+
return aInputs.barPositionRotations;
169+
}
170+
171+
public void setBarPosition(double positionRotations, double velocityRPM) {
172+
173+
positionRotations = MathUtil.clamp(positionRotations, -1000, 1000);
174+
ampBar.setPositionSetpoint(
175+
positionRotations,
176+
barFFmodel.calculate(Math.toRadians(positionRotations), Math.toRadians(velocityRPM)));
177+
}
178+
179+
public void stopAmpBar() {
180+
ampBar.stop();
181+
}
182+
183+
public void setBarGoal(double setpoint) {
184+
this.barGoalPos = setpoint;
185+
barGoal = new TrapezoidProfile.State(setpoint, 0);
186+
}
187+
188+
public void setbarCurrent(double current) {
189+
190+
barCurrent = new TrapezoidProfile.State(current, 0);
191+
}
192+
101193
public void setExtenderGoal(double setpoint) {
102194
goal = setpoint;
103195
extenderGoal = new TrapezoidProfile.State(setpoint, 0);
@@ -133,13 +225,22 @@ public void periodic() {
133225
Logger.recordOutput("Alliance", DriverStation.getAlliance().isPresent());
134226

135227
elevator.updateInputs(eInputs);
228+
ampBar.updateInputs(aInputs);
136229

137230
extenderCurrent =
138231
extenderProfile.calculate(Constants.LOOP_PERIOD_SECS, extenderCurrent, extenderGoal);
139232

233+
barCurrent = barProfile.calculate(Constants.LOOP_PERIOD_SECS, barCurrent, barGoal);
234+
140235
setPositionExtend(extenderCurrent.position, extenderCurrent.velocity);
141236

237+
setBarPosition(barCurrent.position, barCurrent.velocity);
238+
142239
Logger.processInputs("Elevator", eInputs);
240+
Logger.processInputs("Amp bar inputs", aInputs);
241+
242+
Logger.recordOutput("amp bar error", getBarError());
243+
Logger.recordOutput("amp bar goal", barGoalPos);
143244

144245
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode())) {
145246
elevator.configurePID(kP.get(), kI.get(), 0);

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,11 +7,11 @@ public interface AmpBarIO {
77
@AutoLog
88
public static class AmpBarIOInputs {
99

10-
public double barVelocity = 0;
11-
public double barPosition = 0;
10+
public double barVelocityRPM = 0;
11+
public double barPositionRotations = 0;
1212
public double currentAmps = 0;
1313
public double appliedVolts = 0;
14-
public double barPositionSetpoint = 0;
14+
public double barPositionSetpointRotations = 0;
1515
}
1616

1717
public default void updateInputs(AmpBarIOInputs inputs) {}

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

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
package frc.robot.subsystems.elevator;
2+
3+
import edu.wpi.first.math.controller.PIDController;
4+
import edu.wpi.first.math.system.plant.DCMotor;
5+
6+
public class AmpBarIOSIm implements AmpBarIO {
7+
8+
private final DCMotor barGearBox = DCMotor.getNeo550(1);
9+
// private final LinearSystemSim sim =
10+
// new Li
11+
12+
private final PIDController pid = new PIDController(0, 0, 0);
13+
14+
private double currentAmps = 0.0;
15+
private double appliedVolts = 0.0;
16+
private double velocityRPM = 0.0;
17+
private double positionRotations = 0.0;
18+
private double positionSetpointRotations = 0.0;
19+
20+
@Override
21+
public void updateInputs(AmpBarIOInputs inputs) {
22+
positionSetpointRotations = pid.getSetpoint();
23+
24+
// appliedVolts += MathUtil.clamp(pid.calculate(sim.get), velocityRPM, appliedVolts)
25+
26+
}
27+
}

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

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ public class AmpBarIOSparkMAX implements AmpBarIO {
1111

1212
private final CANSparkMax barMotor;
1313
private final SparkPIDController pid;
14-
private double barPositionSetpoint = 0;
14+
private double barPositionSetpoint;
1515

1616
public AmpBarIOSparkMAX(int motorID) {
1717

@@ -22,15 +22,18 @@ public AmpBarIOSparkMAX(int motorID) {
2222

2323
barMotor.restoreFactoryDefaults();
2424
barMotor.setCANTimeout(250);
25+
barMotor.setSmartCurrentLimit(20);
2526
barMotor.burnFlash();
2627
barMotor.clearFaults();
28+
29+
barPositionSetpoint = 0;
2730
}
2831

2932
@Override
3033
public void updateInputs(AmpBarIOInputs inputs) {
31-
inputs.barVelocity = barMotor.getEncoder().getVelocity();
32-
inputs.barPosition = barMotor.getEncoder().getPosition();
33-
inputs.barPositionSetpoint = barPositionSetpoint;
34+
inputs.barVelocityRPM = barMotor.getEncoder().getVelocity();
35+
inputs.barPositionRotations = barMotor.getEncoder().getPosition();
36+
inputs.barPositionSetpointRotations = barPositionSetpoint;
3437
inputs.currentAmps = barMotor.getOutputCurrent();
3538
inputs.appliedVolts = barMotor.getAppliedOutput();
3639
}

0 commit comments

Comments
 (0)