@@ -40,7 +40,7 @@ public class ElevatorSubsystem extends SubsystemBase {
4040
4141 private final TelemetryCANSparkMax elevatorMotor =
4242 new TelemetryCANSparkMax (ELEVATOR_MOTOR_ID , MotorType .kBrushless , "/elevator/motor" , true );
43- private final TelemetryCANSparkMax elevatorMotorFollower = new TelemetryCANSparkMax (ELEVATOR_FOLLOWER_MOTOR_ID , MotorType .kBrushless , "/elevator/followerMotor" , false );
43+ private final TelemetryCANSparkMax elevatorMotorFollower = new TelemetryCANSparkMax (ELEVATOR_FOLLOWER_MOTOR_ID , MotorType .kBrushless , "/elevator/followerMotor" , true );
4444
4545
4646 private final TunableTelemetryProfiledPIDController controller =
@@ -50,10 +50,12 @@ public class ElevatorSubsystem extends SubsystemBase {
5050 private ElevatorFeedforward feedforward = FF_GAINS .createElevatorFeedforward ();
5151
5252 private final RelativeEncoder elevatorEncoder = elevatorMotor .getEncoder ();
53+ private final RelativeEncoder elevatorFollowerEncoder = elevatorMotorFollower .getEncoder ();
5354 private final DigitalInput bottomLimit = new DigitalInput (ELEVATOR_LIMIT_SWITCH );
5455
5556 private final DoubleTelemetryEntry voltageReq =
5657 new DoubleTelemetryEntry ("/elevator/voltageReq" , MiscConstants .TUNING_MODE );
58+
5759
5860 private final BooleanTelemetryEntry isHomedEntry =
5961 new BooleanTelemetryEntry ("/elevator/isHomed" , true );
@@ -62,13 +64,15 @@ public class ElevatorSubsystem extends SubsystemBase {
6264 private final EventTelemetryEntry elevatorEventEntry =
6365 new EventTelemetryEntry ("/elevator/main/events" );
6466 private final EventTelemetryEntry elevatorFollowerEventEntry = new EventTelemetryEntry ("elevator/follower/events" );
67+ private final DoubleTelemetryEntry followerVoltageReq = new DoubleTelemetryEntry ("/elevator/followerReq" , true );
6568
6669 private boolean isHomed = false ;
6770
6871 public ElevatorSubsystem () {
6972 configMotors ();
7073 controller .setTolerance (Units .inchesToMeters (0.5 ));
7174 setDefaultCommand (setVoltageCommand (0.0 ));
75+ // elevatorMotorFollower.follow(elevatorMotor);
7276 }
7377
7478 private void configMotors () {
@@ -130,38 +134,55 @@ private void configMotors() {
130134
131135
132136 ConfigurationUtils .applyCheckRecordRev (
133- () -> elevatorMotorFollower .setCANTimeout (250 ),
134- () -> true ,
135- followerFaultRecorder .run ("CAN timeout" ),
136- MiscConstants .CONFIGURATION_ATTEMPTS );
137+ () -> elevatorMotorFollower .setCANTimeout (250 ),
138+ () -> true ,
139+ faultRecorder .run ("CAN timeout" ),
140+ MiscConstants .CONFIGURATION_ATTEMPTS );
137141 ConfigurationUtils .applyCheckRecordRev (
138- elevatorMotorFollower ::restoreFactoryDefaults ,
139- () -> true ,
140- followerFaultRecorder .run ("Factory default" ),
141- MiscConstants .CONFIGURATION_ATTEMPTS );
142+ elevatorMotorFollower ::restoreFactoryDefaults ,
143+ () -> true ,
144+ faultRecorder .run ("Factory default" ),
145+ MiscConstants .CONFIGURATION_ATTEMPTS );
142146 ConfigurationUtils .applyCheckRecord (
143- () -> elevatorMotorFollower .setInverted (FOLLOWER_INVERTED ),
144- () -> elevatorMotorFollower .getInverted () == FOLLOWER_INVERTED ,
145- () -> elevatorFollowerEventEntry .append ("Motor invert" ),
146- MiscConstants .CONFIGURATION_ATTEMPTS );
147+ () -> elevatorMotorFollower .setInverted (MAIN_INVERTED ),
148+ () -> elevatorMotorFollower .getInverted () == MAIN_INVERTED ,
149+ () -> elevatorEventEntry .append ("Motor invert" ),
150+ MiscConstants .CONFIGURATION_ATTEMPTS );
151+ ConfigurationUtils .applyCheckRecordRev (
152+ () -> elevatorMotorFollower .setSmartCurrentLimit (STALL_MOTOR_CURRENT , FREE_MOTOR_CURRENT ),
153+ () -> true ,
154+ faultRecorder .run ("Current limit" ),
155+ MiscConstants .CONFIGURATION_ATTEMPTS );
156+ ConfigurationUtils .applyCheckRecordRev (
157+ () -> elevatorMotorFollower .setIdleMode (IdleMode .kBrake ),
158+ () -> true ,
159+ faultRecorder .run ("Idle mode" ),
160+ MiscConstants .CONFIGURATION_ATTEMPTS );
147161 ConfigurationUtils .applyCheckRecordRev (
148- () -> elevatorMotorFollower .setSmartCurrentLimit (STALL_MOTOR_CURRENT , FREE_MOTOR_CURRENT ),
149- () -> true ,
150- followerFaultRecorder .run ("Current limit" ),
151- MiscConstants .CONFIGURATION_ATTEMPTS );
162+ () -> elevatorFollowerEncoder .setPositionConversionFactor (METERS_PER_REV ),
163+ () ->
164+ ConfigurationUtils .fpEqual (
165+ elevatorFollowerEncoder .getPositionConversionFactor (), METERS_PER_REV ),
166+ faultRecorder .run ("Position conversion factor" ),
167+ MiscConstants .CONFIGURATION_ATTEMPTS );
152168 ConfigurationUtils .applyCheckRecordRev (
153- () -> elevatorMotorFollower .setIdleMode (IdleMode .kBrake ),
154- () -> true ,
155- followerFaultRecorder .run ("Idle mode" ),
156- MiscConstants .CONFIGURATION_ATTEMPTS );
169+ () -> elevatorFollowerEncoder .setVelocityConversionFactor (METERS_PER_REV / 60 ),
170+ () ->
171+ ConfigurationUtils .fpEqual (
172+ elevatorFollowerEncoder .getVelocityConversionFactor (), METERS_PER_REV / 60 ),
173+ faultRecorder .run ("Velocity conversion factor" ),
174+ MiscConstants .CONFIGURATION_ATTEMPTS );
157175 ConfigurationUtils .applyCheckRecordRev (
158- elevatorMotorFollower ::burnFlashWithDelay ,
159- () -> true ,
160- followerFaultRecorder .run ("Burn flash" ),
161- MiscConstants .CONFIGURATION_ATTEMPTS );
176+ elevatorMotorFollower ::burnFlashWithDelay ,
177+ () -> true ,
178+ faultRecorder .run ("Burn flash" ),
179+ MiscConstants .CONFIGURATION_ATTEMPTS );
180+
181+
162182
163183
164- elevatorMotorFollower .follow (elevatorMotor , FOLLOWER_INVERTED );
184+
185+ // elevatorMotorFollower.follow(elevatorMotor, FOLLOWER_INVERTED);
165186 ConfigurationUtils .postDeviceConfig (
166187 followerFaultRecorder .hasFault (),
167188 elevatorEventEntry ::append ,
@@ -178,13 +199,20 @@ public boolean atBottomLimit() {
178199 public boolean atGoal () {
179200 return controller .atGoal ();
180201 }
202+ public Command setFollowerCommand (double voltage ){
203+ return this .run (() -> elevatorMotorFollower .setVoltage (voltage ));
204+ }
181205
182206 public void setEncoderPosition (double position ) {
183207 elevatorEncoder .setPosition (position );
208+ elevatorFollowerEncoder .setPosition (position );
209+
184210 }
185211
186212 public void setVoltage (double voltage ) {
187213 elevatorMotor .setVoltage (voltage );
214+ elevatorMotorFollower .setVoltage (voltage );
215+ followerVoltageReq .append (voltage );
188216 voltageReq .append (voltage );
189217 }
190218
@@ -194,12 +222,15 @@ public double getPosition() {
194222
195223 public void stopMove () {
196224 elevatorMotor .setVoltage (0 );
225+ elevatorMotorFollower .setVoltage (0 );
197226 }
198227
199228 public boolean isHomed () {
200229 return isHomed ;
201230 }
202231
232+
233+
203234 public Command setElevatorPositionCommand (double position ) {
204235 double positionClamped = MathUtil .clamp (position , ELEVATOR_MIN_HEIGHT , ELEVATOR_MAX_HEIGHT );
205236 return RaiderCommands .ifCondition (this ::isHomed )
@@ -220,7 +251,7 @@ public Command setElevatorPositionCommand(double position) {
220251 }
221252
222253 public Command probeHomeCommand () {
223- return setVoltageCommand (-0.5 ).unless (this ::isHomed ).until (this ::isHomed );
254+ return setVoltageCommand (-0.75 ).unless (this ::isHomed ).until (this ::isHomed );
224255 }
225256
226257 public Command setVoltageCommand (double voltage ) {
0 commit comments