Skip to content

Commit b9880cd

Browse files
abhobeabhobe
committed
Merge branch 'robot-too'
2 parents 3518c5d + 472b70f commit b9880cd

File tree

7 files changed

+64
-42
lines changed

7 files changed

+64
-42
lines changed

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

Lines changed: 14 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -283,15 +283,16 @@ public static final class HANG { // Adjust these as needed
283283
public static final Angle STOW_ANGLE = Degrees.of(110.0);
284284
}
285285

286+
public static final boolean SAFETIES_ENABLED = true;
286287
public static final class ELEVATOR {
287288
public static final double GEARING = (7.5);
288-
public static final Distance CYCLE_HEIGHT = Inches.of(2.15 * Math.PI); // CALCULATE
289+
public static final Distance CYCLE_HEIGHT = Inches.of(2.16 * Math.PI); // CALCULATE
289290
public static final Distance TOLERANCE = Inches.of(0.5);
290291
public static final Distance Bhobe_HEIGHT = Inches.of(1);
291292

292293
public static final class PROFILE {
293-
public static final double kP = 16.0;
294-
public static final double kS = 2.0;
294+
public static final double kP = 4.5;
295+
public static final double kS = 0.1;
295296
}
296297

297298
// HEIGHT IS MEASURED FROM THE GROUND TO THE TOP OF THE ELEVATOR
@@ -305,7 +306,7 @@ public static final class CORAL {
305306
public static final Distance L1_HEIGHT = Inches.of(46.0); //change
306307
public static final Distance L2_HEIGHT = Inches.of(48.50);
307308
public static final Distance L3_HEIGHT = Inches.of(56.5);
308-
public static final Distance L4_HEIGHT = Inches.of(71.0);
309+
public static final Distance L4_HEIGHT = Inches.of(60.0);
309310
public static final Distance INTAKE_HEIGHT = Inches.of(58.80);
310311
}
311312

@@ -322,7 +323,7 @@ public static final class MANIPULATOR_PIVOT {
322323
public static final double GEARING = (36.0/16.0)*(4.0)*(4.0);
323324
public static final double ROTATION_DELAY = 0.3; // seconds
324325
public static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.25);
325-
public static final Angle ABSOLUTE_POSITION_OFFSET = Rotations.of(0.0);
326+
public static final Angle ABSOLUTE_POSITION_OFFSET = Rotations.of(0.652);
326327
public static final Angle CENTER_OF_MASS_OFFSET = Degrees.of(69.134); // CALCULATED FROM CAD
327328

328329
public static final class PROFILE {
@@ -333,10 +334,10 @@ public static final class PROFILE {
333334
public static final double MAX_ACCELERATION = 30.0; // rad/s^2
334335
}
335336

336-
public static final Angle MAX_ANGLE = Degrees.of(36.0); // RESET TO 40.0
337-
public static final Angle MIN_ANGLE = Degrees.of(-135.5);
337+
public static final Angle MAX_ANGLE = Degrees.of(0.0); // RESET TO 40.0
338+
public static final Angle MIN_ANGLE = Degrees.of(-90.0);
338339

339-
public static final Angle STOW_ANGLE = Degrees.of(38.5);
340+
public static final Angle STOW_ANGLE = Degrees.of(-5.0);
340341
public static final Angle SAFE_ANGLE = Degrees.of(-30.0);
341342

342343
public static final Angle SAFE_MIN_ANGLE = Degrees.of(-45);
@@ -345,13 +346,14 @@ public static final class PROFILE {
345346
public static final Angle PID_MIN_ANGLE = Degrees.of(-90.0);
346347
public static final Angle PID_MID_ANGLE = Degrees.of(-45.0);
347348
public static final Angle PID_MAX_ANGLE = Degrees.of(0.0);
348-
public static final Angle TOLERANCE = Degrees.of(2.0);
349+
public static final Angle TOLERANCE = Degrees.of(1.0);
349350
public static final Angle SAFE_TOLERANCE = Degrees.of(4.0);
351+
public static final boolean INVERTED = true;
350352

351353
public static final class CORAL {
352-
public static final Angle L1_ANGLE = Degrees.of(22.5);
353-
public static final Angle L23_ANGLE = Degrees.of(-34.5);
354-
public static final Angle L4_ANGLE = Degrees.of(-57);
354+
public static final Angle L1_ANGLE = Degrees.of(-10.0);
355+
public static final Angle L23_ANGLE = Degrees.of(-20.0);
356+
public static final Angle L4_ANGLE = Degrees.of(-30.0);
355357
public static final Angle INTAKE_ANGLE = Degrees.of(-27.992817);
356358
}
357359

@@ -384,9 +386,6 @@ public static final class ALGAE {
384386
}
385387

386388
public static final class MANIPULATOR {
387-
public static final boolean INVERT_ALGAE_LEFT = true;
388-
public static final boolean INVERT_ALGAE_RIGHT = true;
389-
390389
public static final Current ALGAE_DETECT_CURRENT = Amps.of(15);
391390
public static final Time ALGAE_GRIP_CHECK_TIME = Seconds.of(0.25);
392391
public static final Time ALGAE_GRIP_CHECK_RATE = Seconds.of(2.0);

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

Lines changed: 21 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
import edu.wpi.first.wpilibj2.command.Command;
66
import edu.wpi.first.wpilibj2.command.Commands;
77
import edu.wpi.first.wpilibj2.command.SubsystemBase;
8+
import frc.robot.Constants.Constants;
89
import frc.robot.Constants.Constants.MANIPULATOR_PIVOT;
910
import frc.robot.subsystems.elevator.Elevator;
1011
import frc.robot.subsystems.manipulator.Manipulator;
@@ -36,20 +37,33 @@ public static Angle calcSafeMaxAngle(Distance elevatorHeight) {
3637
}
3738

3839
public Command safeMoveCommand(Command elevatorCommand, Command manipulatorCommand) {
39-
return Commands.sequence(manipulator.pivot.safe(), elevatorCommand, manipulatorCommand);
40+
if (Constants.SAFETIES_ENABLED) {
41+
return Commands.sequence(
42+
manipulator.pivot.safe(),
43+
elevatorCommand,
44+
manipulatorCommand);
45+
}
46+
return Commands.sequence(elevatorCommand, manipulatorCommand);
4047
}
4148

4249
public Command parallelSafeCommand(Command elevatorCommand, Command manipulatorCommand) {
43-
return Commands.sequence(
50+
if (Constants.SAFETIES_ENABLED) {
51+
return Commands.sequence(
4452
manipulator.pivot.safe(), Commands.parallel(elevatorCommand, manipulatorCommand));
53+
}
54+
55+
return Commands.parallel(elevatorCommand, manipulatorCommand);
56+
4557
}
4658

4759
@Override
4860
public void periodic() {
49-
Distance elevatorHeight = elevator.getAverageHeight();
50-
Angle minAngle = calcSafeMinAngle(elevatorHeight);
51-
Angle maxAngle = calcSafeMaxAngle(elevatorHeight);
52-
53-
manipulator.pivot.setMinMaxAngle(minAngle, maxAngle);
61+
if (Constants.SAFETIES_ENABLED) {
62+
Distance elevatorHeight = elevator.getAverageHeight();
63+
Angle minAngle = calcSafeMinAngle(elevatorHeight);
64+
Angle maxAngle = calcSafeMaxAngle(elevatorHeight);
65+
66+
manipulator.pivot.setMinMaxAngle(minAngle, maxAngle);
67+
}
5468
}
5569
}

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

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -78,16 +78,22 @@ public static void configureBindings(
7878
// L3 Algae Removal Height
7979
// Algae ground Height
8080

81-
operator.a().onTrue(pieceCombos.safeRaise());
81+
operator.a().onTrue(pieceCombos.coralL1());
8282
operator.b().onTrue(pieceCombos.coralL2());
8383
operator.x().onTrue(pieceCombos.coralL3());
8484
operator.y().onTrue(pieceCombos.coralL4());
85+
86+
// operator.a().onTrue(elevator.coralL1());
87+
// operator.b().onTrue(elevator.coralL2());
88+
// operator.x().onTrue(elevator.coralL3());
89+
// operator.y().onTrue(elevator.coralL4());
90+
8591
// operator.y().onTrue(manipulator.pivot.safe().andThen(elevator.coralL4().alongWith(manipulator.pivot.coralL4())));
8692

8793
// operator.y().onTrue(elevator.algaeBarge().andThen(manipulator.pivot.algaeBarge()));
88-
// operator.a().onTrue(manipulator.pivot.coralL23());
89-
// operator.b().onTrue(manipulator.pivot.algaeReef());
90-
// operator.x().onTrue(manipulator.pivot.coralIntake());
94+
// operator.a().onTrue(manipulator.pivot.coralL1());
95+
// operator.b().onTrue(manipulator.pivot.coralL23());
96+
// operator.x().onTrue(manipulator.pivot.coralL4());
9197
// operator.y().onTrue(manipulator.pivot.stow());
9298
// operator.start().onTrue(pieceCombos.stow()); // assume this is processor height
9399
// operator.back().onTrue(elevator.algaeGround());

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ public Command hold() {
7878
() -> {
7979
Distance position = getAverageHeight();
8080

81-
return run(() -> setHeight(position));
81+
return setHeight(position);
8282
},
8383
Set.of(this));
8484
}

‎src/main/java/frc/robot/subsystems/manipulator/pivot/RealManipulatorPivot.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ public RealManipulatorPivot() {
3737
MANIPULATOR_PIVOT.MIN_ANGLE,
3838
MANIPULATOR_PIVOT.MAX_ANGLE,
3939
MANIPULATOR_PIVOT.TOLERANCE,
40-
false);
40+
MANIPULATOR_PIVOT.INVERTED);
4141
// setDefaultCommand(stow());
4242

4343
// setDefaultCommand(pivotTo(() -> stopAngle));

‎src/main/java/frc/robot/util/hardware/motion/DualLinearActuator.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -189,7 +189,7 @@ public Command up() {
189189
}
190190

191191
public Command down() {
192-
return move(-0.2);
192+
return move(-0.1);
193193
}
194194

195195
public boolean unsafeMoveDown() {

‎src/main/java/frc/robot/util/hardware/motion/PivotController.java‎

Lines changed: 16 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
import static edu.wpi.first.units.Units.Radians;
55
import static edu.wpi.first.units.Units.Rotations;
66

7+
import com.revrobotics.AbsoluteEncoder;
78
import com.revrobotics.RelativeEncoder;
89
import com.revrobotics.spark.ClosedLoopSlot;
910
import com.revrobotics.spark.SparkBase.ControlType;
@@ -46,7 +47,7 @@ public class PivotController extends SubsystemBase {
4647
// Built-in relative NEO encoder
4748
protected RelativeEncoder encoder;
4849
// Rev absolute through-bore encoder
49-
protected DutyCycleEncoder absoluteEncoder;
50+
protected AbsoluteEncoder absoluteEncoder;
5051

5152
private Angle minAngle, maxAngle;
5253

@@ -88,11 +89,11 @@ public PivotController(
8889

8990
// motor.getAbsoluteEncoder();
9091

91-
absoluteEncoder = new DutyCycleEncoder(absoluteEncoderDIO, 1.0, absolutePositionOffset);
92+
// absoluteEncoder = new DutyCycleEncoder(absoluteEncoderDIO, 1.0, absolutePositionOffset);
9293

93-
// absoluteEncoder = motor.getAbsoluteEncoder();
94+
absoluteEncoder = motor.getAbsoluteEncoder();
9495
// motorConfig.absoluteEncoder.zeroCentered(false);
95-
// motorConfig.absoluteEncoder.zeroOffset(absolutePositionOffset);
96+
motorConfig.absoluteEncoder.zeroOffset(absolutePositionOffset);
9697

9798

9899
this.kS = kS;
@@ -109,8 +110,8 @@ public PivotController(
109110
SparkMaxUtil.saveAndLog(this, motor, motorConfig);
110111

111112
StatusChecks.Category statusChecks = StatusChecks.under(this);
112-
statusChecks.add("absoluteEncoderConnected", () -> absoluteEncoder.isConnected());
113-
statusChecks.add("absoluteEncoderUpdated", () -> absoluteEncoder.get() != 0.0);
113+
// statusChecks.add("absoluteEncoderConnected", () -> absoluteEncoder.isConnected());
114+
statusChecks.add("absoluteEncoderUpdated", () -> absoluteEncoder.getPosition() != 0.0);
114115
statusChecks.add("motor", motor);
115116

116117
sim =
@@ -128,7 +129,9 @@ public PivotController(
128129
Logger.logBoolean(this.getName() + "/doneMoving", this::doneMoving);
129130
Logger.logBoolean(this.getName() + "/safety/forward", this::triggeredForwardSafety);
130131
Logger.logBoolean(this.getName() + "/safety/reverse", this::triggeredReverseSafety);
131-
Logger.logNumber(this.getName() + "/appliedDutyCycle", () -> motor.getAppliedOutput());
132+
Logger.logNumber(this.getName() + "/duty/applied", () -> motor.getAppliedOutput());
133+
Logger.logNumber(this.getName() + "/duty/pid", () -> motor.get());
134+
132135

133136
Logger.logNumber(this.getName() + "/angle/target", () -> getTargetAngle().in(Rotations));
134137
Logger.logNumber(this.getName() + "/angle/relative", () -> getRelativePosition().in(Rotations));
@@ -137,7 +140,7 @@ public PivotController(
137140
Logger.logNumber(this.getName() + "/angle/min", () -> getMinAngle().in(Rotations));
138141
Logger.logNumber(this.getName() + "/angle/max", () -> getMaxAngle().in(Rotations));
139142

140-
Logger.logBoolean(this.getName() + "/encoderConnected", absoluteEncoder::isConnected);
143+
// Logger.logBoolean(this.getName() + "/encoderConnected", absoluteEncoder::isConnected);
141144

142145
RobotContainer.disabledPeriodic.subscribe(this::disabledPeriodic);
143146
}
@@ -155,10 +158,10 @@ public void moveTowards(Angle requestedAngle) {
155158
if (requestedAngle == null) return; // If we havent set a target angle yet, do nothing
156159
targetAngle = clampAngle(requestedAngle);
157160

158-
if (!absoluteEncoder.isConnected()) {
159-
motor.stopMotor();
160-
return;
161-
}
161+
// if (!absoluteEncoder.isConnected()) {
162+
// motor.stopMotor();
163+
// return;
164+
// }
162165

163166
// encoder.setPosition(getPosition().in(Rotations));
164167

@@ -279,7 +282,7 @@ public Angle getRelativePosition() {
279282
}
280283

281284
public Angle getRawAbsolutePosition() {
282-
return Rotations.of(absoluteEncoder.get());
285+
return Rotations.of(absoluteEncoder.getPosition());
283286
}
284287

285288
public Angle getAbsolutePosition() {

0 commit comments

Comments
 (0)