Skip to content

Commit 31f39a6

Browse files
dabeycorndabeycorn
committed
dirty
1 parent 9187aaa commit 31f39a6

File tree

9 files changed

+137
-103
lines changed

9 files changed

+137
-103
lines changed

‎src/main/java/wmironpatriots/Robot.java‎

Lines changed: 48 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,7 @@ public class Robot extends TimedRobot implements Logged {
5656
private final Chute chute;
5757

5858
private final RobotVisualizer visualizer;
59+
5960
private final SendableChooser<Command> autonChooser;
6061

6162
public Robot() {
@@ -150,23 +151,52 @@ public Robot() {
150151
.signedPow(2.0)
151152
.scale(maxAngularSpeed);
152153

153-
swerve.setDefaultCommand(swerve.teleopSwerveCmmd(x, y, omega));
154+
// swerve.setDefaultCommand(swerve.teleopSwerveCmmd(x, y, omega));
154155

155156
elevator.setDefaultCommand(
156157
Commands.sequence(
157158
elevator.runPoseZeroingCmmd().onlyIf(() -> !elevator.isZeroed()),
158-
elevator.setTargetPoseCmmd(Elevator.IDLE).until(() -> elevator.inSetpointRange()),
159+
elevator
160+
.setTargetPoseCmmd(1.0)
161+
.until(
162+
() ->
163+
elevator.inSetpointRange() || elevator.getSetpoint() > elevator.getPose()),
159164
elevator.stopMotorInputCmmd()));
160165

161166
tail.setDefaultCommand(
162167
Commands.sequence(
163168
tail.runPoseZeroingCmmd()
164169
.onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
165-
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
166-
.until(() -> Superstructure.isTailSafe(elevator, tail)),
167-
tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
168-
tail.stopMotorInputCmmd()));
169-
170+
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
171+
// .until(() -> Superstructure.isTailSafe(elevator, tail)),
172+
// tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
173+
// tail.stopMotorInputCmmd()));
174+
175+
// driveController
176+
// .a()
177+
// .whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED))
178+
// .onFalse(chute.runChuteSpeedCmmd(0.0));
179+
180+
// driveController.a().whileTrue(tail.setTargetPoseCmmd(0.0));
181+
182+
// driveController.x().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS));
183+
driveController
184+
.x()
185+
.whileTrue(tail.setRollerSpeedCmmd(1.4))
186+
.whileFalse(tail.setRollerSpeedCmmd(0));
187+
188+
driveController
189+
.a()
190+
.whileTrue(tail.setRollerSpeedCmmd(-1.4))
191+
.whileFalse(tail.setRollerSpeedCmmd(0));
192+
193+
driveController
194+
.y()
195+
.whileTrue(chute.runChuteSpeedCmmd(-1).alongWith(tail.setRollerSpeedCmmd(1.3)))
196+
.whileFalse(chute.runChuteSpeedCmmd(0).alongWith(tail.setRollerSpeedCmmd(0)));
197+
198+
driveController.b().whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L3));
199+
// driveController.x().whileTrue(tail.setRollerSpeedCmmd(1)).onFalse(tail.setRollerSpeedCmmd(0.0));
170200
// * SUPERSTRUCTURE INIT
171201
Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
172202

@@ -189,24 +219,24 @@ public Robot() {
189219
@Override
190220
public void robotPeriodic() {
191221
CommandScheduler.getInstance().run();
192-
if (Robot.isSimulation()) {
193-
SimulatedArena.getInstance().simulationPeriodic();
194-
}
222+
// if (Robot.isSimulation()) {
223+
// SimulatedArena.getInstance().simulationPeriodic();
224+
// }
195225

196226
Monologue.updateAll();
197227
visualizer.periodic();
198228
}
199229

200230
@Override
201231
public void autonomousInit() {
202-
Command auton =
203-
autonChooser
204-
.getSelected()
205-
.withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
206-
207-
if (auton != null) {
208-
auton.schedule();
209-
}
232+
// Command auton =
233+
// autonChooser
234+
// .getSelected()
235+
// .withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
236+
237+
// if (auton != null) {
238+
// auton.schedule();
239+
// }
210240
}
211241

212242
@Override

‎src/main/java/wmironpatriots/subsystems/chute/Chute.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1111

1212
public abstract class Chute extends SubsystemBase {
13-
public static final double INTAKE_SPEED = 5.0;
13+
public static final double INTAKE_SPEED = -0.13;
1414

1515
public Command runChuteSpeedCmmd(double speed) {
1616
return this.run(() -> {});

‎src/main/java/wmironpatriots/subsystems/elevator/Elevator.java‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ public abstract class Elevator extends SubsystemBase implements Logged {
3030
public static final double POSE_L1 = 0.0;
3131
public static final double POSE_L2 = 3.16;
3232
public static final double POSE_L3 = 10.81;
33-
public static final double POSE_L4 = 24;
33+
public static final double POSE_L4 = 11;
3434

3535
// Visualizer constants
3636
public static final double MAX_ELEVATOR_HEIGHT_INCHES = 57;
@@ -89,6 +89,7 @@ public Command runPoseZeroingCmmd() {
8989
stopMotors();
9090
resetPose();
9191
isZeroed = true;
92+
System.out.println("Elevator Zeroed");
9293
});
9394
}
9495

@@ -109,17 +110,21 @@ private void resetPose() {
109110

110111
/** Returns pose in revs */
111112
public double getPose() {
112-
return poseRevs;
113+
return parentPoseRevs;
113114
}
114115

115116
/** Gets velocity in rotations per minute */
116117
public double getVelocity() {
117118
return velRPM;
118119
}
119120

121+
public double getSetpoint() {
122+
return setpointPoseRots;
123+
}
124+
120125
/** Gets displacement from setpoint pose */
121126
public double getSetpointDisplacement() {
122-
return setpointPoseRots - poseRevs;
127+
return setpointPoseRots - parentPoseRevs;
123128
}
124129

125130
/** Checks if elevator has been zeroed */
@@ -129,7 +134,7 @@ public boolean isZeroed() {
129134

130135
/** Checks if elevator is around a specific range of the setpoint */
131136
public boolean inSetpointRange() {
132-
return Math.abs(setpointPoseRots - parentPoseRevs) < 0.05; // TODO tweak range if needed
137+
return Math.abs(setpointPoseRots - parentPoseRevs) < 0.5; // TODO tweak range if needed
133138
}
134139

135140
/** HARDWARE METHODS */

‎src/main/java/wmironpatriots/subsystems/elevator/ElevatorIOComp.java‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -14,16 +14,18 @@
1414
import com.ctre.phoenix6.controls.PositionVoltage;
1515
import com.ctre.phoenix6.controls.VoltageOut;
1616
import com.ctre.phoenix6.hardware.TalonFX;
17+
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
18+
import com.ctre.phoenix6.signals.GravityTypeValue;
1719
import com.ctre.phoenix6.signals.InvertedValue;
1820
import com.ctre.phoenix6.signals.NeutralModeValue;
21+
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
1922
import edu.wpi.first.math.util.Units;
2023

2124
public class ElevatorIOComp extends Elevator {
2225
private final TalonFX parent, child;
2326
private final TalonFXConfiguration motorConf;
2427

25-
private final PositionVoltage reqMotorPose =
26-
new PositionVoltage(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
28+
private final PositionVoltage reqPoseVolts = new PositionVoltage(0.0).withEnableFOC(true);
2729

2830
private final VoltageOut reqMotorVolt =
2931
new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0);
@@ -52,10 +54,13 @@ public ElevatorIOComp() {
5254

5355
motorConf = new TalonFXConfiguration();
5456
motorConf.MotorOutput.NeutralMode = NeutralModeValue.Brake;
55-
motorConf.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
57+
motorConf.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
58+
motorConf.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
5659

5760
motorConf.Slot0.withKP(1).withKI(0.0).withKD(0); // PID config
5861
motorConf.Slot0.withKG(0.59).withKS(0.0).withKV(4.12).withKA(0.0); // feedforward config
62+
motorConf.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
63+
motorConf.Slot0.GravityType = GravityTypeValue.Elevator_Static;
5964

6065
motorConf.CurrentLimits.StatorCurrentLimit = 80.0;
6166
motorConf.CurrentLimits.StatorCurrentLimitEnable = true;
@@ -66,10 +71,6 @@ public ElevatorIOComp() {
6671
motorConf.TorqueCurrent.PeakForwardTorqueCurrent = 80.0;
6772
motorConf.TorqueCurrent.PeakReverseTorqueCurrent = -80.0;
6873

69-
motorConf.MotionMagic.MotionMagicCruiseVelocity = 0.03; // ! TODO
70-
motorConf.MotionMagic.MotionMagicAcceleration = 0.03;
71-
motorConf.MotionMagic.MotionMagicJerk = 0.0;
72-
7374
parent.getConfigurator().apply(motorConf);
7475
child.getConfigurator().apply(motorConf);
7576

@@ -153,7 +154,7 @@ protected void runMotorVolts(double volts) {
153154

154155
@Override
155156
protected void runMotorPose(double poseRevs) {
156-
parent.setControl(reqMotorPose.withEnableFOC(true).withPosition(poseRevs));
157+
parent.setControl(reqPoseVolts.withEnableFOC(true).withPosition(poseRevs));
157158
}
158159

159160
@Override

‎src/main/java/wmironpatriots/subsystems/swerve/Swerve.java‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -136,10 +136,7 @@ public void periodic() {
136136
updateOdo();
137137
f2d.setRobotPose(getPose());
138138
publisher.set(getModuleStates());
139-
140-
if (DriverStation.isDisabled()) {
141-
stop();
142-
}
139+
stop();
143140
}
144141
}
145142

‎src/main/java/wmironpatriots/subsystems/swerve/module/Module.java‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -32,13 +32,13 @@ public abstract class Module extends IronComponent {
3232
// static methods
3333
protected static TalonFXConfiguration getPivotConfig() {
3434
TalonFXConfiguration config = new TalonFXConfiguration();
35-
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
35+
config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
3636

3737
config.CurrentLimits.SupplyCurrentLimit = 20.0;
3838
config.CurrentLimits.SupplyCurrentLimitEnable = true;
3939

4040
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
41-
config.Feedback.SensorToMechanismRatio = PIVOT_REDUCTION;
41+
// config.Feedback.SensorToMechanismRatio = PIVOT_REDUCTION;
4242
config.Slot0.kP = 20.0;
4343
config.Slot0.kI = 0.0;
4444
config.Slot0.kD = 0.68275;
@@ -60,11 +60,11 @@ protected static TalonFXConfiguration getDriveConfig() {
6060
config.CurrentLimits.StatorCurrentLimit = 120.0;
6161
config.CurrentLimits.StatorCurrentLimitEnable = true;
6262

63-
config.Feedback.SensorToMechanismRatio = DRIVE_REDUCTION;
63+
// config.Feedback.SensorToMechanismRatio = DRIVE_REDUCTION;
6464
// TODO TUNE VALUES
65-
config.Slot0.kA = 5.0;
66-
config.Slot0.kS = 10.0;
67-
config.Slot0.kP = 300.0;
65+
config.Slot0.kA = 0.33291;
66+
config.Slot0.kS = 0.088468;
67+
config.Slot0.kP = 3.2;
6868
config.Slot0.kD = 0.0;
6969

7070
return config;
@@ -136,7 +136,7 @@ public void moduleCoastingEnabled(boolean enabled) {
136136
}
137137

138138
public Rotation2d getCurrentAngle() {
139-
return Rotation2d.fromRadians(pivotPoseRads);
139+
return Rotation2d.fromRotations(pivotPoseRads);
140140
}
141141

142142
public SwerveModulePosition getModulePose() {

‎src/main/java/wmironpatriots/subsystems/swerve/module/ModuleIOComp.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -122,8 +122,8 @@ public void periodic() {
122122

123123
double rotsToRads = (2 * Math.PI);
124124

125-
pivotABSPoseRads = cancoderPose.getValueAsDouble() * rotsToRads;
126-
pivotPoseRads = pivotPose.getValueAsDouble() * rotsToRads;
125+
pivotABSPoseRads = cancoderPose.getValueAsDouble();
126+
pivotPoseRads = pivotPose.getValueAsDouble();
127127
pivotVelRadsPerSec = pivotVel.getValueAsDouble() * rotsToRads;
128128
pivotAppliedVolts = pivotVolts.getValueAsDouble();
129129
pivotSupplyCurrent = pivotSCurrent.getValueAsDouble();

‎src/main/java/wmironpatriots/subsystems/tail/Tail.java‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,10 @@
77
package wmironpatriots.subsystems.tail;
88

99
import edu.wpi.first.wpilibj2.command.Command;
10-
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1110
import monologue.Annotations.Log;
11+
import wmironpatriots.util.mechanismUtil.IronSubsystem;
1212

13-
public abstract class Tail extends SubsystemBase {
13+
public abstract class Tail implements IronSubsystem {
1414
/** CONSTANTS */
1515
// mech constants
1616
// TODO check values in CAD
@@ -22,14 +22,14 @@ public abstract class Tail extends SubsystemBase {
2222
public static final double CURRENT_LIMIT = 40.0;
2323

2424
// Poses
25-
public static final double POSE_MIN_REVS = -7.357;
26-
public static final double POSE_MAX_REVS = 0;
25+
public static final double POSE_MIN_REVS = -10.4;
26+
public static final double POSE_MAX_REVS = 0.1;
2727

2828
// Roller speeds
2929
public static final double INTAKING_SPEEDS = 5;
3030
public static final double OUTTAKING_SPEEDS = 5;
3131

32-
public static final double PIVOT_P = 0.3;
32+
public static final double PIVOT_P = 1.5;
3333
public static final double PIVOT_I = 0.0;
3434
public static final double PIVOT_D = 0.0;
3535

@@ -73,18 +73,19 @@ public Command zeroPoseCmmd() {
7373
return this.run(
7474
() -> {
7575
isZeroed = true;
76-
setEncoderPose(POSE_MAX_REVS);
76+
// setEncoderPose(POSE_MAX_REVS);
7777
});
7878
}
7979

8080
/** Runs pivot backwards until current spikes above threshold */
8181
public Command runPoseZeroingCmmd() {
82-
return this.run(() -> runPivotVolts(0.5))
82+
return this.run(() -> runPivotVolts(-1))
8383
.until(() -> pivotSupplyCurrentAmps > 20.0)
8484
.finallyDo(
8585
(interrupted) -> {
8686
runPivotVolts(0.0);
8787
setEncoderPose(POSE_MAX_REVS);
88+
System.out.println("Tail zeroed");
8889
isZeroed = true;
8990
});
9091
}

0 commit comments

Comments
 (0)