Skip to content

Commit 4001cdc

Browse files
nlaverdureDylanTaylor29
nlaverdure
and
DylanTaylor29
authored
Swerve working (#96)
* initial test of robot * changed camera names * configure rev devices asynchronously * update camera transforms * added method for initializing relative turning encoders * added a work around to turning encoder offset * decreased turning p value * formatting --------- Co-authored-by: DylanTaylor29 <[email protected]>
1 parent b0246cc commit 4001cdc

File tree

12 files changed

+55
-39
lines changed

12 files changed

+55
-39
lines changed

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -170,7 +170,7 @@ public static final class DriveControllerGains {
170170
}
171171

172172
public static final class TurningControllerGains {
173-
public static final double kP = 10.0; // 1.5;
173+
public static final double kP = 1.5; // 1.5;
174174
public static final double kI = 0.0; // 2023 Competition Robot
175175
public static final double kD = 0.0; // 2023 Competition Robot
176176
}
@@ -282,13 +282,13 @@ public static final class ClimberConstants {
282282
// Convert to inches per second at the winch
283283
public static final double kVelocityConversionFactor = kPositionConversionFactor / 60.0;
284284

285-
public static final double kMaxVelocityInchesPerSecond = 10.0;
285+
public static final double kMaxVelocityInchesPerSecond = 5.0;
286286

287287
public static final double kMaxVelocityRPM =
288288
kMaxVelocityInchesPerSecond / kVelocityConversionFactor;
289289
public static final double kMaxAccelerationRPMPerSecond = kMaxVelocityRPM; // 100% accel in 1s
290290

291-
public static final double kDeployPosition = 12.0; // inches
291+
public static final double kDeployPosition = 7.0; // inches
292292
}
293293

294294
public static final class LedConstants {

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

Lines changed: 17 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,6 @@
4141
import frc.robot.elevator.CoralRoller;
4242
import frc.robot.elevator.CoralWrist;
4343
import frc.robot.elevator.Elevator;
44-
import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
4544
import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
4645
import frc.robot.elevator.Lifter;
4746
import frc.robot.vision.Vision;
@@ -202,16 +201,18 @@ private void configureDriverButtonBindings() {
202201

203202
// Reset heading
204203
driver.DIn()
205-
.onTrue(new InstantCommand(() -> swerve.setHeadingOffset())
206-
.ignoringDisable(true));
204+
.onTrue(new InstantCommand(() -> {
205+
swerve.setHeadingOffset();
206+
// swerve.initializeRelativeTurningEncoder();
207+
}).ignoringDisable(true));
207208

208209
// Drive to nearest pose
209210
driver.AIn()
210211
.whileTrue(new DriveToPoseCommand(swerve, vision, () -> swerve.getNearestPose()));
211212

212213
// Outtake grippers
213214
driver.HIn()
214-
.onTrue(coralRoller.createOuttakeCommand()
215+
.whileTrue(coralRoller.createOuttakeCommand()
215216
.alongWith(algaeRoller.createOuttakeCommand()));
216217

217218
}
@@ -221,12 +222,12 @@ private void configureOperatorButtonBindings() {
221222
Trigger algaeMode = operator.leftBumper();
222223

223224
// Test wrist motion
224-
operator.back()
225-
.onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode)
226-
.alongWith(algaeWrist.createSetAngleCommand(AlgaeWristState.Floor)));
227-
operator.start()
228-
.onTrue(coralWrist.createSetAngleCommand(CoralWristState.L4)
229-
.alongWith(algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode)));
225+
// operator.back()
226+
// .onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode)
227+
// .alongWith(algaeWrist.createSetAngleCommand(AlgaeWristState.Floor)));
228+
// operator.start()
229+
// .onTrue(coralWrist.createSetAngleCommand(CoralWristState.L4)
230+
// .alongWith(algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode)));
230231

231232
// Test algae roller motion
232233
// operator.back().whileTrue(algaeRoller.createIntakeCommand());
@@ -253,16 +254,12 @@ private void configureOperatorButtonBindings() {
253254
elevator.algaeBargePositionCG(), elevator.coralL4PositionCG(), algaeMode));
254255

255256
// Configure to either intake coral from source or intake algae from floor
256-
// operator.start().whileTrue(new ConditionalCommand(
257-
// elevator.algaeFloorIntakeCG(), elevator.coralIntakeCG(), algaeMode));
258-
259-
// Intake with coral gripper
260-
operator.rightBumper().and(lifter.atIntakingHeight)
261-
.whileTrue(coralRoller.createIntakeCommand());
262-
263-
// Intake with algae gripper
264-
operator.rightBumper().and(lifter.atIntakingHeight.negate())
265-
.whileTrue(algaeRoller.createIntakeCommand());
257+
operator.start().whileTrue(new ConditionalCommand(
258+
elevator.algaeFloorIntakeCG(), elevator.coralIntakeCG(), algaeMode));
259+
260+
// Intake either coral or algae
261+
operator.rightBumper().whileTrue(new ConditionalCommand(
262+
algaeRoller.createIntakeCommand(), coralRoller.createIntakeCommand(), algaeMode));
266263

267264
// Force joystick operation of the elevator
268265
Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);

‎src/main/java/frc/robot/climber/Climber.java

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,8 @@ public Climber() {
6060
.reverseSoftLimitEnabled(true);
6161
// spotless:on
6262

63-
motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
63+
motor.configureAsync(
64+
motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
6465

6566
encoder = motor.getEncoder();
6667
controller = motor.getClosedLoopController();

‎src/main/java/frc/robot/drivetrain/Drivetrain.java

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,8 @@ public void periodic() {
8989
m_visionPosePublisher.set(poseEstimator.getEstimatedPosition());
9090

9191
for (SwerveModule module : SwerveModule.values()) {
92+
module.refreshRelativeTurningEncoder();
93+
9294
SmartDashboard.putNumber(
9395
module.getName() + "/RelativeTurningPosition",
9496
module.getRelativeTurningPosition().getDegrees());
@@ -158,6 +160,12 @@ public void setPose(Pose2d pose) {
158160
poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
159161
}
160162

163+
public void initializeRelativeTurningEncoder() {
164+
for (SwerveModule module : SwerveModule.values()) {
165+
module.initializeRelativeTurningEncoder();
166+
}
167+
}
168+
161169
public Rotation2d getHeadingOffset() {
162170
return headingOffset;
163171
}

‎src/main/java/frc/robot/drivetrain/SwerveModule.java

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -104,9 +104,9 @@ private SwerveModule(
104104
.primaryEncoderVelocityPeriodMs(100);
105105
// spotless:on
106106

107-
m_driveMotor.configure(
107+
m_driveMotor.configureAsync(
108108
m_driveMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
109-
m_turningMotor.configure(
109+
m_turningMotor.configureAsync(
110110
m_turningMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
111111

112112
m_driveController = m_driveMotor.getClosedLoopController();
@@ -200,6 +200,10 @@ public void initializeRelativeTurningEncoder() {
200200
m_turningRelativeEncoder.setPosition(getAbsTurningPosition(0.25).getRotations());
201201
}
202202

203+
public void refreshRelativeTurningEncoder() {
204+
m_turningRelativeEncoder.setPosition(getAbsTurningPosition(0.0).getRotations());
205+
}
206+
203207
/** Initializes the magnetic offset of the absolute turning encoder */
204208
public void initializeAbsoluteTurningEncoder() {
205209
double magnetOffsetFromCANCoder = getAbsTurningEncoderOffset().getRotations();

‎src/main/java/frc/robot/elevator/AlgaeRoller.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,9 @@ public AlgaeRoller() {
4949
.follow(leaderMotor, true);
5050
// spotless:on
5151

52-
leaderMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
53-
followerMotor.configure(
52+
leaderMotor.configureAsync(
53+
config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
54+
followerMotor.configureAsync(
5455
followerConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
5556

5657
setDefaultCommand(createStopCommand());

‎src/main/java/frc/robot/elevator/AlgaeWrist.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@ public AlgaeWrist() {
7272
.forwardSoftLimitEnabled(true);
7373
// spotless:on
7474

75-
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
75+
motor.configureAsync(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
7676

7777
feedback.setTolerance(AlgaeWristConstants.kAllowableError.in(Radians));
7878
feedback.setIZone(AlgaeWristConstants.kIZone.in(Radians));

‎src/main/java/frc/robot/elevator/CoralRoller.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ public CoralRoller() {
3030
.voltageCompensation(RobotConstants.kNominalVoltage)
3131
.idleMode(IdleMode.kCoast)
3232
.smartCurrentLimit(RobotConstants.kDefaultNEO550CurretnLimit)
33-
.inverted(false);
33+
.inverted(true);
3434

3535
config.signals
3636
.absoluteEncoderPositionPeriodMs(100)
@@ -41,7 +41,7 @@ public CoralRoller() {
4141
.positionConversionFactor(CoralRollerConstants.kPositionConversionFactor);
4242
// spotless:on
4343

44-
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
44+
motor.configureAsync(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
4545

4646
setDefaultCommand(createStopCommand());
4747
}

‎src/main/java/frc/robot/elevator/CoralWrist.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ public CoralWrist() {
7575
// .forwardSoftLimitEnabled(false);
7676
// spotless:on
7777

78-
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
78+
motor.configureAsync(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
7979

8080
feedback.setTolerance(CoralWristConstants.kAllowableError.in(Radians));
8181
feedback.setIZone(CoralWristConstants.kIZone.in(Radians));

‎src/main/java/frc/robot/elevator/ElevatorConstants.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ public static enum LifterState {
5454
CoralL2(13.8),
5555
CoralL3(30.0),
5656
CoralL4(60.75),
57-
CoralIntake(14.7),
57+
CoralIntake(7.7),
5858
AlgaeProcessor(12.0),
5959
AlgaeL2(30),
6060
AlgaeL3(46.5),

0 commit comments

Comments
 (0)