Skip to content

Commit 7ff1c1b

Browse files
Vignesh1234587Vignesh1234587
committed
changed units to rotations and setAmpBarTarget command
1 parent 4f43aca commit 7ff1c1b

6 files changed

Lines changed: 95 additions & 44 deletions

File tree

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,12 @@ public final class BuildConstants {
55
public static final String MAVEN_GROUP = "";
66
public static final String MAVEN_NAME = "2024 Robot Code";
77
public static final String VERSION = "unspecified";
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";
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";
1111
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;
12+
public static final String BUILD_DATE = "2024-08-10 13:30:10 EDT";
13+
public static final long BUILD_UNIX_TIME = 1723311010234L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

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

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
import frc.robot.commands.PositionNoteInFeeder;
4747
import frc.robot.commands.ScoreAmp;
4848
import frc.robot.commands.ScoreTrap;
49+
import frc.robot.commands.SetAmpBarTarget;
4950
import frc.robot.commands.SetElevatorTarget;
5051
import frc.robot.commands.SetPivotTarget;
5152
import frc.robot.commands.SetShooterTargetRPM;
@@ -709,8 +710,8 @@ private void testControls() {
709710
driveController.leftTrigger().onTrue(new InstantCommand(() -> shooter.setFeedersRPM(500)));
710711
driveController.leftTrigger().onFalse(new InstantCommand(() -> shooter.stopFeeders()));
711712

712-
driveController.b().onTrue(new InstantCommand(() -> elevator.setBarGoal(1000), elevator));
713-
driveController.b().onFalse(new InstantCommand(() -> elevator.setBarGoal(0), elevator));
713+
driveController.b().onTrue(new SetAmpBarTarget(10, 0, elevator));
714+
driveController.b().onFalse(new SetAmpBarTarget(0, 0, elevator));
714715
}
715716

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

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

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands;
6+
7+
import edu.wpi.first.wpilibj2.command.Command;
8+
import frc.robot.subsystems.elevator.Elevator;
9+
10+
public class SetAmpBarTarget extends Command {
11+
/** Creates a new SetAmpBarTarget. */
12+
private final Elevator elevator;
13+
14+
private double threshold;
15+
private double setPoint;
16+
17+
public SetAmpBarTarget(double setpoint, double threshold, Elevator elevator) {
18+
// Use addRequirements() here to declare subsystem dependencies.
19+
this.setPoint = setPoint;
20+
this.threshold = threshold;
21+
this.elevator = elevator;
22+
23+
addRequirements(elevator);
24+
}
25+
26+
// Called when the command is initially scheduled.
27+
@Override
28+
public void initialize() {
29+
elevator.setBarGoal(setPoint);
30+
}
31+
32+
// Called every time the scheduler runs while the command is scheduled.
33+
@Override
34+
public void execute() {}
35+
36+
// Called once the command ends or is interrupted.
37+
@Override
38+
public void end(boolean interrupted) {
39+
elevator.stopAmpBar();
40+
}
41+
42+
// Returns true when the command should end.
43+
@Override
44+
public boolean isFinished() {
45+
return elevator.ampBarAtSetpoint();
46+
}
47+
}

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

Lines changed: 30 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -28,18 +28,18 @@ public class Elevator extends SubsystemBase {
2828

2929
// amp bar gains
3030

31-
private static double barkP;
32-
private static double barkV;
33-
private static double barkG;
31+
private static final LoggedTunableNumber barkP = new LoggedTunableNumber("Bar/kP");
32+
private static final LoggedTunableNumber barkV = new LoggedTunableNumber("Bar/kV");
33+
private static final LoggedTunableNumber barkG = new LoggedTunableNumber("Bar/kG");
3434

3535
private TrapezoidProfile extenderProfile;
3636
private TrapezoidProfile.Constraints extenderConstraints =
3737
new TrapezoidProfile.Constraints(30, 85);
3838
private TrapezoidProfile.State extenderGoal = new TrapezoidProfile.State();
3939
private TrapezoidProfile.State extenderCurrent = new TrapezoidProfile.State();
4040

41-
private static double maxVelocityRPM;
42-
private static double maxAccelerationRPM;
41+
private static double maxVelocityDegPerSec;
42+
private static double maxAccelerationDegPerSecSquared;
4343

4444
private TrapezoidProfile barProfile;
4545
private TrapezoidProfile.Constraints barConstraints;
@@ -66,9 +66,9 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
6666
kP.initDefault(0.44);
6767
kI.initDefault(0);
6868

69-
barkP = 0.0;
70-
barkV = 0.0;
71-
barkG = 0.0;
69+
barkP.initDefault(0);
70+
barkV.initDefault(0);
71+
barkG.initDefault(0);
7272
break;
7373
case REPLAY:
7474
kS.initDefault(0);
@@ -79,9 +79,9 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
7979
kP.initDefault(15);
8080
kI.initDefault(0);
8181

82-
barkP = 0.0;
83-
barkV = 0.0;
84-
barkG = 0.0;
82+
barkP.initDefault(0);
83+
barkV.initDefault(0);
84+
barkG.initDefault(0);
8585
break;
8686
case SIM:
8787
kS.initDefault(0);
@@ -92,9 +92,9 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
9292
kP.initDefault(1);
9393
kI.initDefault(0);
9494

95-
barkP = 0.0;
96-
barkV = 0.0;
97-
barkG = 0.0;
95+
barkP.initDefault(0);
96+
barkV.initDefault(0);
97+
barkG.initDefault(0);
9898
break;
9999
default:
100100
kS.initDefault(0);
@@ -105,36 +105,37 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
105105
kP.initDefault(15);
106106
kI.initDefault(0);
107107

108-
barkP = 0.0;
109-
barkV = 0.0;
110-
barkG = 0.0;
108+
barkP.initDefault(0);
109+
barkV.initDefault(0);
110+
barkG.initDefault(0);
111111
break;
112112
}
113113

114114
setExtenderGoal(0.162);
115115
extenderProfile = new TrapezoidProfile(extenderConstraints);
116116
extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
117117

118-
maxVelocityRPM = 15;
119-
maxAccelerationRPM = 1;
118+
maxVelocityDegPerSec = 15;
119+
maxAccelerationDegPerSecSquared = 1;
120120

121-
barConstraints = new TrapezoidProfile.Constraints(maxVelocityRPM, maxAccelerationRPM);
121+
barConstraints =
122+
new TrapezoidProfile.Constraints(maxVelocityDegPerSec, maxAccelerationDegPerSecSquared);
122123
barProfile = new TrapezoidProfile(barConstraints);
123124
barCurrent = barProfile.calculate(0, barCurrent, barGoal);
124125

125126
this.elevator.configurePID(kP.get(), 0, 0);
126127
elevatorFFModel = new ElevatorFeedforward(kS.get(), kG.get(), kV.get(), kA.get());
127128

128-
ampBar.configurePID(barkP, 0, 0);
129-
barFFmodel = new ArmFeedforward(0, barkG, barkV, 0);
129+
ampBar.configurePID(barkP.get(), 0, 0);
130+
barFFmodel = new ArmFeedforward(0, barkG.get(), barkV.get(), 0);
130131
}
131132

132133
public boolean atGoal() {
133134
return (Math.abs(eInputs.elevatorPosition - goal) <= Constants.ElevatorConstants.THRESHOLD);
134135
}
135136

136137
public boolean barAtGoal() {
137-
return (Math.abs(aInputs.barPositionRotations - barGoalPos)
138+
return (Math.abs(aInputs.barPositionDegrees - barGoalPos)
138139
<= Constants.ElevatorConstants.BAR_THRESHOLD);
139140
}
140141

@@ -148,7 +149,7 @@ private double getElevatorError() {
148149

149150
private double getBarError() {
150151

151-
return aInputs.barPositionSetpointRotations - aInputs.barPositionRotations;
152+
return aInputs.barPositionSetpointDegrees - aInputs.barPositionDegrees;
152153
}
153154

154155
public boolean elevatorAtSetpoint() {
@@ -165,15 +166,16 @@ public void setBarBrakeMode(boolean bool) {
165166
}
166167

167168
public double getBarPositionRotations() {
168-
return aInputs.barPositionRotations;
169+
return aInputs.barPositionDegrees;
169170
}
170171

171-
public void setBarPosition(double positionRotations, double velocityRPM) {
172+
public void setBarPosition(double positionRotations, double velocityDegsPerSec) {
172173

173-
positionRotations = MathUtil.clamp(positionRotations, -1000, 1000);
174+
positionRotations = MathUtil.clamp(positionRotations, 0, 20);
174175
ampBar.setPositionSetpoint(
175176
positionRotations,
176-
barFFmodel.calculate(Math.toRadians(positionRotations), Math.toRadians(velocityRPM)));
177+
barFFmodel.calculate(
178+
Math.toRadians(positionRotations), Math.toRadians(velocityDegsPerSec)));
177179
}
178180

179181
public void stopAmpBar() {

‎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 barVelocityRPM = 0;
11-
public double barPositionRotations = 0;
10+
public double barVelocityDegsPerSec = 0;
11+
public double barPositionDegrees = 0;
1212
public double currentAmps = 0;
1313
public double appliedVolts = 0;
14-
public double barPositionSetpointRotations = 0;
14+
public double barPositionSetpointDegrees = 0;
1515
}
1616

1717
public default void updateInputs(AmpBarIOInputs inputs) {}

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

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -31,9 +31,9 @@ public AmpBarIOSparkMAX(int motorID) {
3131

3232
@Override
3333
public void updateInputs(AmpBarIOInputs inputs) {
34-
inputs.barVelocityRPM = barMotor.getEncoder().getVelocity();
35-
inputs.barPositionRotations = barMotor.getEncoder().getPosition();
36-
inputs.barPositionSetpointRotations = barPositionSetpoint;
34+
inputs.barVelocityDegsPerSec = (barMotor.getEncoder().getVelocity() / 15) * 6;
35+
inputs.barPositionDegrees = (barMotor.getEncoder().getPosition() / 15) * 360;
36+
inputs.barPositionSetpointDegrees = barPositionSetpoint;
3737
inputs.currentAmps = barMotor.getOutputCurrent();
3838
inputs.appliedVolts = barMotor.getAppliedOutput();
3939
}
@@ -49,10 +49,11 @@ public void setBrakeMode(boolean bool) {
4949
}
5050

5151
@Override
52-
public void setPositionSetpoint(double position, double ffVolts) {
52+
public void setPositionSetpoint(double positionDegs, double ffVolts) {
5353

54-
this.barPositionSetpoint = position;
55-
pid.setReference(position, ControlType.kPosition, 0, ffVolts, ArbFFUnits.kVoltage);
54+
this.barPositionSetpoint = positionDegs;
55+
pid.setReference(
56+
(positionDegs / 360) * 15, ControlType.kPosition, 0, ffVolts, ArbFFUnits.kVoltage);
5657
}
5758

5859
@Override

0 commit comments

Comments
 (0)