Skip to content

Commit add8c1b

Browse files
ohowe1ohowe1
committed
Merge branch 'workingongettingready' of https://github.com/RegisJesuitRobotics/Competition2024 into workingongettingready
2 parents b303a4e + 600c486 commit add8c1b

File tree

9 files changed

+54
-104
lines changed

9 files changed

+54
-104
lines changed

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

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -24,16 +24,16 @@ public static class ScoringConstants {
2424

2525
public static class IntakeConstants {
2626
public static final int INTAKE_VOLTAGE = 3;
27-
public static final int INTAKE_MOTOR_ID = 15;
27+
public static final int INTAKE_MOTOR_ID = 12;
2828

2929
public static final int STALL_MOTOR_CURRENT = 40;
3030
public static final int FREE_MOTOR_CURRENT = 20;
3131
}
3232

3333
public static class SlapdownConstants {
3434

35-
public static final int FEEDER_MOTOR_ID = 16;
36-
public static final int ROTATION_MOTOR_ID = 17;
35+
public static final int FEEDER_MOTOR_ID = 14;
36+
public static final int ROTATION_MOTOR_ID = 13;
3737
public static final double ROTATION_GEAR_RATIO = 5.0 * 3.0 * 30.0 / 15.0;
3838

3939
public static final int FEED_STALL_MOTOR_CURRENT = 20;
@@ -60,7 +60,7 @@ public static class ElevatorConstants {
6060
public static final int STALL_MOTOR_CURRENT = 45;
6161
public static final int FREE_MOTOR_CURRENT = 25;
6262

63-
public static final int ELEVATOR_MOTOR_ID = 14;
63+
public static final int ELEVATOR_MOTOR_ID = 15;
6464

6565
public static final int ELEVATOR_LIMIT_SWITCH = 2;
6666

@@ -146,18 +146,18 @@ private SwerveConstants() {}
146146

147147
public static final SwerveModuleConfiguration FRONT_LEFT_MODULE_CONFIGURATION =
148148
new SwerveModuleConfiguration(
149-
1, 4, 9, false, true, -0.7455146908760071, false, SHARED_SWERVE_MODULE_CONFIGURATION);
149+
1, 5, 17, false, true, -0.7455146908760071, false, SHARED_SWERVE_MODULE_CONFIGURATION);
150150
public static final SwerveModuleConfiguration FRONT_RIGHT_MODULE_CONFIGURATION =
151151
new SwerveModuleConfiguration(
152-
2, 6, 10, false, true, 2.7857091108003242, false, SHARED_SWERVE_MODULE_CONFIGURATION);
152+
2, 6, 18, false, true, 2.7857091108003242, false, SHARED_SWERVE_MODULE_CONFIGURATION);
153153

154154
public static final SwerveModuleConfiguration BACK_LEFT_MODULE_CONFIGURATION =
155155
new SwerveModuleConfiguration(
156-
3, 7, 11, false, true, 2.676796474860444, false, SHARED_SWERVE_MODULE_CONFIGURATION);
156+
3, 7, 19, false, true, 2.676796474860444, false, SHARED_SWERVE_MODULE_CONFIGURATION);
157157

158158
public static final SwerveModuleConfiguration BACK_RIGHT_MODULE_CONFIGURATION =
159159
new SwerveModuleConfiguration(
160-
4, 8, 12, false, true, -2.4605051837685683, false, SHARED_SWERVE_MODULE_CONFIGURATION);
160+
4, 8, 20, false, true, -2.4605051837685683, false, SHARED_SWERVE_MODULE_CONFIGURATION);
161161
}
162162

163163
public static class AutoConstants {
@@ -197,7 +197,7 @@ public static class WristConstants {
197197
public static final Rotation2d WRIST_MAX = new Rotation2d(Units.degreesToRadians(60));
198198
public static final Rotation2d WRIST_MIN = new Rotation2d(0);
199199

200-
public static final int WRIST_MOTOR_ID = 11;
200+
public static final int WRIST_MOTOR_ID = 9;
201201
public static final int STALL_MOTOR_CURRENT = 45;
202202
public static final int FREE_MOTOR_CURRENT = 25;
203203

@@ -212,7 +212,7 @@ public static class WristConstants {
212212
}
213213

214214
public static class TransportConstants {
215-
public static final int TRANSPORT_MOTOR_ID = 0;
215+
public static final int TRANSPORT_MOTOR_ID = 11;
216216
public static final int STALL_MOTOR_CURRENT = 45;
217217
public static final int FREE_MOTOR_CURRENT = 25;
218218
public static final double TRANSPORT_VOLTAGE = 2.0;
@@ -222,7 +222,7 @@ public static class ShooterConstants {
222222
public static final int FREE_MOTOR_CURRENT = 25;
223223
public static final int STALL_MOTOR_CURRENT = 45;
224224

225-
public static final int SHOOTER_ID = 18;
225+
public static final int SHOOTER_ID = 10;
226226

227227
public static final double SHOOTING_RPM = 3000.0;
228228

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,8 @@ public class FieldConstants {
3535
public static final class StagingLocations {
3636
public static double centerlineX = fieldLength / 2.0;
3737
// TODO: FIX THIS
38-
public static final double stagingThreshold = 0;
39-
public static final double ampThreshold = 0;
38+
public static final double stagingThreshold = 5;
39+
public static final double ampThreshold = 5;
4040

4141
// need to update
4242
public static double centerlineFirstY = Units.inchesToMeters(29.638);

‎src/main/java/frc/robot/commands/elevator/AmpPlaceCommand.java‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,10 @@
11
package frc.robot.commands.elevator;
22

3-
import static frc.robot.Constants.ElevatorConstants.*;
3+
import static frc.robot.Constants.ScoringConstants.*;
44
import static frc.robot.Constants.WristConstants.*;
55

66
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
77
import frc.robot.commands.shooter.ShootAtAngleCommand;
8-
import frc.robot.commands.wrist.WristToPositionCommand;
98
import frc.robot.subsystems.elevator.ElevatorSubsystem;
109
import frc.robot.subsystems.shooter.ShooterSubsystem;
1110
import frc.robot.subsystems.transport.TransportSubsystem;
@@ -27,8 +26,8 @@ public AmpPlaceCommand(
2726
this.wristSubsystem = wristSubsystem;
2827
this.transportSubsystem = transportSubsystem;
2928
addCommands(
30-
new ElevatorToPositionCommand(elevatorSubsystem, ELEVATOR_AMP_POS),
31-
new WristToPositionCommand(wristSubsystem, WRIST_AMP_POSITION),
29+
elevatorSubsystem.setElevatorPositionCommand(AMP_ELEVATOR_HEIGHT),
30+
wristSubsystem.setPosiitonCommand(WRIST_AMP_POSITION),
3231
new ShootAtAngleCommand(
3332
shooterSubsystem, transportSubsystem, wristSubsystem, WRIST_AMP_POSITION));
3433
}

‎src/main/java/frc/robot/commands/elevator/ElevatorToPositionCommand.java‎

Lines changed: 0 additions & 34 deletions
This file was deleted.

‎src/main/java/frc/robot/commands/elevator/ElevatorWristParallelCommand.java‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,8 @@
11
package frc.robot.commands.elevator;
22

3-
import static frc.robot.Constants.ElevatorConstants.*;
43
import static frc.robot.Constants.WristConstants.*;
54

65
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
7-
import frc.robot.commands.wrist.WristToPositionCommand;
86
import frc.robot.subsystems.elevator.ElevatorSubsystem;
97
import frc.robot.subsystems.wrist.WristSubsystem;
108

@@ -18,7 +16,7 @@ public ElevatorWristParallelCommand(
1816
this.wristSubsystem = wristSubsystem;
1917

2018
addCommands(
21-
new WristToPositionCommand(wristSubsystem, WRIST_MIN),
22-
new ElevatorToPositionCommand(elevatorSubsystem, 0));
19+
wristSubsystem.setPosiitonCommand(WRIST_MIN),
20+
elevatorSubsystem.setElevatorPositionCommand(0));
2321
}
2422
}

‎src/main/java/frc/robot/commands/shooter/RunWristAndFlywheels.java‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@
44

55
import edu.wpi.first.math.geometry.Rotation2d;
66
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
7-
import frc.robot.commands.wrist.WristToPositionCommand;
87
import frc.robot.subsystems.shooter.ShooterSubsystem;
98
import frc.robot.subsystems.wrist.WristSubsystem;
109

@@ -19,7 +18,6 @@ public RunWristAndFlywheels(
1918
this.shooterSubsystem = shooterSubsystem;
2019

2120
addCommands(
22-
new WristToPositionCommand(wristSubsystem, shootingAngle),
23-
shooterSubsystem.runRPMCommand(SHOOTING_RPM));
21+
wristSubsystem.setPosiitonCommand(shootingAngle), shooterSubsystem.runFlyRPM(SHOOTING_RPM));
2422
}
2523
}

‎src/main/java/frc/robot/commands/wrist/WristToPositionCommand.java‎

Lines changed: 0 additions & 38 deletions
This file was deleted.

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

Lines changed: 26 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import com.revrobotics.RelativeEncoder;
88
import edu.wpi.first.math.MathUtil;
99
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
10+
import edu.wpi.first.math.trajectory.TrapezoidProfile;
1011
import edu.wpi.first.wpilibj.DigitalInput;
1112
import edu.wpi.first.wpilibj2.command.Command;
1213
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -88,6 +89,12 @@ private void configMotors() {
8889
elevatorAlert.set(faultInitializing);
8990
}
9091

92+
public void atBottomLimit() {
93+
if (bottomLimit.get()) {
94+
elevatorEncoder.setPosition(0);
95+
}
96+
}
97+
9198
public void setDesiredPosition(double desiredPosition) {
9299
// TODO: CLAMP THIS
93100
controller.setGoal(desiredPosition);
@@ -97,6 +104,10 @@ public boolean atGoal() {
97104
return controller.atGoal();
98105
}
99106

107+
public void setEncoderPosition(double position) {
108+
elevatorEncoder.setPosition(position);
109+
}
110+
100111
public void setVoltage(double voltage) {
101112
elevatorMotor.setVoltage(voltage);
102113
}
@@ -106,7 +117,7 @@ public double getPosition() {
106117
}
107118

108119
public void stopMove() {
109-
setVoltage(0.0);
120+
elevatorMotor.setVoltage(0);
110121
}
111122

112123
// TODO: Fix
@@ -124,19 +135,27 @@ public Command runElevatorCommand(double voltage) {
124135

125136
// TODO: Fix
126137
public Command setElevatorPositionCommand(double position) {
127-
return this.run(() -> this.setDesiredPosition(position));
138+
return this.run(
139+
() -> {
140+
controller.setGoal(position);
141+
double feedbackOutput = controller.calculate(getPosition());
142+
TrapezoidProfile.State currentSetpoint = controller.getSetpoint();
143+
144+
setVoltage(
145+
feedbackOutput + feedforward.calculate(getPosition(), currentSetpoint.velocity));
146+
});
128147
}
129148

130149
@Override
131150
public void periodic() {
132-
// TODO: Log that this is being reset, or maybe don't always reset?
133-
if (bottomLimit.get()) {
134-
elevatorEncoder.setPosition(0);
135-
}
151+
logValues();
136152
}
137153

138154
private void logValues() {
139-
bottomLimitEntry.append(bottomLimit.get());
140155
elevatorMotor.logValues();
156+
bottomLimitEntry.append(bottomLimit.get());
157+
if (FF_GAINS.hasChanged()) {
158+
feedforward = FF_GAINS.createFeedforward();
159+
}
141160
}
142161
}

‎src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -143,7 +143,15 @@ private double getPosition() {
143143
}
144144

145145
public Command setRotationGoalCommand(Rotation2d goal) {
146-
return this.run(() -> this.setRotationGoal(goal)).until(rotationController::atGoal);
146+
return this.run(
147+
() -> {
148+
this.setRotationGoal(goal);
149+
double feedbackOutput = rotationController.calculate(getPosition());
150+
TrapezoidProfile.State currentSetpoint = rotationController.getSetpoint();
151+
152+
setRotationVoltage(
153+
feedbackOutput + rotationFF.calculate(getPosition(), currentSetpoint.velocity));
154+
});
147155
}
148156

149157
public Command setFeederVoltageCommand(double voltage) {

0 commit comments

Comments
 (0)