Skip to content

Commit 2c89f93

Browse files
Vignesh1234587Vignesh1234587
committed
amp bar changes
1 parent 3dcdfec commit 2c89f93

7 files changed

Lines changed: 72 additions & 34 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 = "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 = 318;
9-
public static final String GIT_SHA = "e8551a83b11b902aaddd7d48b4ea2b02323d1b44";
10-
public static final String GIT_DATE = "2024-08-10 14:29:45 EDT";
8+
public static final int GIT_REVISION = 319;
9+
public static final String GIT_SHA = "3dcdfecb762813f82838b5f7190d2d615e22aa3d";
10+
public static final String GIT_DATE = "2024-08-10 16:48:53 EDT";
1111
public static final String GIT_BRANCH = "amp-bar";
12-
public static final String BUILD_DATE = "2024-08-10 16:39:27 EDT";
13-
public static final long BUILD_UNIX_TIME = 1723322367710L;
12+
public static final String BUILD_DATE = "2024-08-10 18:59:31 EDT";
13+
public static final long BUILD_UNIX_TIME = 1723330771486L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

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

Lines changed: 33 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -299,9 +299,10 @@ public RobotContainer() {
299299
SHOOT_STATE.AMP,
300300
new SequentialCommandGroup(
301301
// amp shoot
302-
new InstantCommand(() -> shooter.setFeedersRPM(500)),
303-
new WaitCommand(1.323),
304-
new InstantCommand(() -> shooter.setFlywheelRPMs(-900, -900)))),
302+
new InstantCommand(() -> shooter.setFeedersRPM(500))
303+
// new WaitCommand(1.323)
304+
)),
305+
// new InstantCommand(() -> shooter.setFlywheelRPMs(-900, -900)))),
305306
Map.entry(
306307
SHOOT_STATE.TRAP,
307308
new SequentialCommandGroup(
@@ -711,7 +712,18 @@ private void testControls() {
711712
driveController.leftTrigger().onFalse(new InstantCommand(() -> shooter.stopFeeders()));
712713

713714
driveController.b().onTrue(new SetAmpBarTarget(10, 0, elevator));
714-
driveController.b().onFalse(new SetAmpBarTarget(0, 0, elevator));
715+
driveController
716+
.b()
717+
.onFalse(
718+
new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.AIMBOT))
719+
.andThen(
720+
new SequentialCommandGroup(
721+
new SetAmpBarTarget(5, 3, elevator),
722+
new InstantCommand(() -> shooter.turnOffFan()),
723+
new SetElevatorTarget(0, 0.5, elevator),
724+
new InstantCommand(() -> elevator.setConstraints(30, 85)),
725+
new InstantCommand(() -> shooter.stopFlywheels(), shooter),
726+
new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))));
715727
}
716728

717729
// TODO:: change drive controls to match changed test controls
@@ -825,9 +837,24 @@ private void driverControls() {
825837
driveAButton.onTrue(climbCommands);
826838

827839
driveXButton.onTrue(trapCommands);
828-
driveController.b().onTrue(new SetAmpBarTarget(90, 3, elevator));
829-
driveController.b().onFalse(new SetAmpBarTarget(0, 3, elevator));
840+
driveController
841+
.b()
842+
.onTrue(
843+
new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.AMP))
844+
.andThen(new ScoreAmp(elevator, pivot, shooter, drive)));
830845

846+
driveController
847+
.b()
848+
.onFalse(
849+
new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.AIMBOT))
850+
.andThen(
851+
new SequentialCommandGroup(
852+
new SetAmpBarTarget(5, 3, elevator),
853+
new InstantCommand(() -> shooter.turnOffFan()),
854+
new SetElevatorTarget(0, 0.5, elevator),
855+
new InstantCommand(() -> elevator.setConstraints(30, 85)),
856+
new InstantCommand(() -> shooter.stopFlywheels(), shooter),
857+
new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))));
831858
// driveController
832859
// .rightBumper()
833860
// .whileTrue(

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ public static class PivotIDs {
2424
// public static final int LEFT = 14;
2525
// public static final int RIGHT = 15;
2626
public static final int LEFT = 14;
27+
2728
public static final int RIGHT = 15;
2829
}
2930

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

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
package frc.robot.commands;
66

77
import edu.wpi.first.wpilibj2.command.InstantCommand;
8+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
89
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
910
import frc.robot.Constants;
1011
import frc.robot.subsystems.drive.Drive;
@@ -19,10 +20,13 @@ public class ScoreAmp extends SequentialCommandGroup {
1920
/** Creates a new ScoreAmp. */
2021
public ScoreAmp(Elevator elevator, Pivot pivot, Shooter shooter, Drive drive) {
2122
addCommands(
22-
new InstantCommand(() -> elevator.setConstraints(100, 640)),
23-
new InstantCommand(() -> shooter.setFlywheelRPMs(500, 500), shooter),
24-
new SetPivotTarget(Constants.PivotConstants.AMP_SETPOINT_DEG, pivot),
23+
new ParallelCommandGroup(
24+
new InstantCommand(() -> elevator.setConstraints(100, 640), elevator),
25+
new InstantCommand(() -> shooter.setFlywheelRPMs(1200, 1200), shooter),
26+
new SetPivotTarget(Constants.PivotConstants.AMP_SETPOINT_DEG, pivot)),
27+
new SetAmpBarTarget(180, 0, elevator),
2528
new SetElevatorTarget(6, 1, elevator));
29+
2630
// addCommands(
2731
// new ParallelCommandGroup(
2832
// // new AlignToAmp(drive),

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,13 +37,13 @@ public void execute() {}
3737
// Called once the command ends or is interrupted.
3838
@Override
3939
public void end(boolean interrupted) {
40-
elevator.stopAmpBar();
40+
// elevator.stopAmpBar();
4141
}
4242

4343
// Returns true when the command should end.
4444
@Override
4545
public boolean isFinished() {
4646
Logger.recordOutput("bar command ", elevator.ampBarAtGoal());
47-
return elevator.ampBarAtGoal();
47+
return true;
4848
}
4949
}

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

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -65,8 +65,8 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
6565
kP.initDefault(0.44);
6666
kI.initDefault(0);
6767

68-
barkP.initDefault(0.04);
69-
barkV.initDefault(4);
68+
barkP.initDefault(0.25);
69+
barkV.initDefault(0.2);
7070
barkG.initDefault(0);
7171
break;
7272
case REPLAY:
@@ -114,8 +114,8 @@ public Elevator(ElevatorIO elevator, AmpBarIO ampBar) {
114114
extenderProfile = new TrapezoidProfile(extenderConstraints);
115115
extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
116116

117-
maxVelocityDegPerSec = 90;
118-
maxAccelerationDegPerSecSquared = 30;
117+
maxVelocityDegPerSec = 1200;
118+
maxAccelerationDegPerSecSquared = 800;
119119

120120
barConstraints =
121121
new TrapezoidProfile.Constraints(maxVelocityDegPerSec, maxAccelerationDegPerSecSquared);
@@ -172,29 +172,28 @@ public double getBarPositionRotations() {
172172
return aInputs.barPositionDegrees;
173173
}
174174

175-
public void setBarPosition(double positionRotations, double velocityDegsPerSec) {
175+
public void setBarPosition(double positionDegrees, double velocityDegsPerSec) {
176176

177177
// positionRotations = MathUtil.clamp(positionRotations, 0, 20);
178-
ampBar.setPositionSetpoint(
179-
positionRotations,
180-
barFFmodel.calculate(
181-
Math.toRadians(positionRotations), Math.toRadians(velocityDegsPerSec)));
178+
Logger.recordOutput("bar PositionDegrees", positionDegrees);
179+
ampBar.setPositionSetpoint(positionDegrees, 0);
180+
// barFFmodel.calculate(Math.toRadians(positionDegrees), Math.toRadians(velocityDegsPerSec))
182181
}
183182

184183
public void stopAmpBar() {
185184
ampBar.stop();
186185
}
187186

188-
public void setBarGoal(double setpoint) {
187+
public void setBarGoal(double barGoalDegrees) {
189188

190-
barGoal = new TrapezoidProfile.State(setpoint, 0);
191-
Logger.recordOutput("bar goal", setpoint);
189+
barGoal = new TrapezoidProfile.State(barGoalDegrees, 0);
190+
Logger.recordOutput("bar goal", barGoalDegrees);
192191
}
193192

194-
public void setbarCurrent(double current) {
193+
// public void setbarCurrent(double current) {
195194

196-
barCurrent = new TrapezoidProfile.State(current, 0);
197-
}
195+
// barCurrent = new TrapezoidProfile.State(current, 0);
196+
// }
198197

199198
public void setExtenderGoal(double setpoint) {
200199
goal = setpoint;
@@ -248,6 +247,7 @@ public void periodic() {
248247
Logger.recordOutput("amp bar error", getBarError());
249248
Logger.recordOutput("amp bar goal", barGoal.position);
250249

250+
Logger.recordOutput("amp bar currentPos", barCurrent.position);
251251
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode())) {
252252
elevator.configurePID(kP.get(), kI.get(), 0);
253253
}

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

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,14 @@
66
import com.revrobotics.CANSparkMax;
77
import com.revrobotics.SparkPIDController;
88
import com.revrobotics.SparkPIDController.ArbFFUnits;
9+
import org.littletonrobotics.junction.Logger;
910

1011
public class AmpBarIOSparkMAX implements AmpBarIO {
1112

1213
private final CANSparkMax barMotor;
1314
private final SparkPIDController pid;
1415
private double barPositionSetpoint;
15-
private final double gearRatio = 15 / 1;
16+
private final double gearRatio = 5. / 1.;
1617

1718
public AmpBarIOSparkMAX(int motorID) {
1819

@@ -23,8 +24,10 @@ public AmpBarIOSparkMAX(int motorID) {
2324

2425
barMotor.restoreFactoryDefaults();
2526
barMotor.setCANTimeout(250);
26-
barMotor.setSmartCurrentLimit(40);
27+
barMotor.setSmartCurrentLimit(30);
2728
barMotor.getEncoder().setPosition(0);
29+
barMotor.getEncoder().setPositionConversionFactor(1);
30+
barMotor.setInverted(true);
2831

2932
barMotor.burnFlash();
3033
barMotor.clearFaults();
@@ -55,6 +58,9 @@ public void setBrakeMode(boolean bool) {
5558
public void setPositionSetpoint(double barPositionOutputDegs, double ffVolts) {
5659

5760
this.barPositionSetpoint = barPositionOutputDegs;
61+
Logger.recordOutput("barPositionOutputDegs", barPositionOutputDegs);
62+
Logger.recordOutput("bar equation reference", (barPositionOutputDegs / 360.) * gearRatio);
63+
Logger.recordOutput("ffVolts", ffVolts);
5864
pid.setReference(
5965
(barPositionOutputDegs / 360.) * gearRatio,
6066
ControlType.kPosition,

0 commit comments

Comments
 (0)