Skip to content

Commit e0ee7d5

Browse files
Danx3merDanx3mer
committed
Fixed pivot state control mode, used an enum for cleaner and more understandable code.
Made code slightly cleaner by removing a few lines and adhering to java coding conventions Created new subsystem branches, please use these branches for further development of all subsystems including coding the subsystems and testing them.
1 parent 76e0215 commit e0ee7d5

File tree

6 files changed

+64
-39
lines changed

6 files changed

+64
-39
lines changed

‎src/main/java/com/stuypulse/robot/RobotContainer.java‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
import com.stuypulse.robot.commands.pivot.PivotToAlgaeIntake;
1919
import com.stuypulse.robot.commands.pivot.PivotToAlgaeStow;
2020
import com.stuypulse.robot.commands.pivot.PivotToCoralStow;
21-
import com.stuypulse.robot.commands.pivot.SetPivotStateMode;
21+
import com.stuypulse.robot.commands.pivot.SetPivotControlMode;
2222
import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeIntake;
2323
import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeOuttake;
2424
import com.stuypulse.robot.commands.pivot.roller.PivotCoralOuttake;
@@ -28,6 +28,7 @@
2828
import com.stuypulse.robot.subsystems.drivetrain.Drivetrain;
2929
import com.stuypulse.robot.subsystems.leds.LEDController;
3030
import com.stuypulse.robot.subsystems.pivot.Pivot;
31+
import com.stuypulse.robot.subsystems.pivot.Pivot.PivotControlMode;
3132
import com.stuypulse.robot.constants.Ports;
3233

3334
import com.stuypulse.stuylib.input.Gamepad;
@@ -104,13 +105,13 @@ private void configureButtonBindings() {
104105

105106
//DPAD
106107
driver.getDPadRight()
107-
.onTrue(new SetPivotStateMode(true))
108+
.onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
108109
.onTrue(new PivotToAlgaeStow())
109-
.whileFalse(new SetPivotStateMode(false));
110+
.whileFalse(new SetPivotControlMode(PivotControlMode.MANUAL));
110111
driver.getDPadDown()
111-
.onTrue(new SetPivotStateMode(true))
112+
.onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
112113
.onTrue(new PivotToAlgaeIntake())
113-
.whileFalse(new SetPivotStateMode(false));
114+
.whileFalse(new SetPivotControlMode(PivotControlMode.MANUAL));
114115
}
115116

116117
/**************/

‎src/main/java/com/stuypulse/robot/commands/pivot/SetPivotControlMode.java‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
package com.stuypulse.robot.commands.pivot;
2+
3+
import com.stuypulse.robot.subsystems.pivot.Pivot;
4+
import com.stuypulse.robot.subsystems.pivot.Pivot.PivotControlMode;
5+
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
8+
public class SetPivotControlMode extends Command {
9+
Pivot pivot = Pivot.getInstance();
10+
PivotControlMode pivotControlMode;
11+
12+
public SetPivotControlMode (PivotControlMode pivotControlMode) {
13+
this.pivotControlMode = pivotControlMode;
14+
}
15+
16+
@Override
17+
public void initialize() {
18+
pivot.SetPivotControlMode(pivotControlMode);
19+
}
20+
}

‎src/main/java/com/stuypulse/robot/commands/pivot/SetPivotStateMode.java‎

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

‎src/main/java/com/stuypulse/robot/constants/Settings.java‎

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
package com.stuypulse.robot.constants;
66

77
import com.stuypulse.stuylib.network.SmartNumber;
8+
import com.stuypulse.stuylib.network.SmartString;
89

910
import edu.wpi.first.math.geometry.Rotation2d;
1011
import edu.wpi.first.units.Units;
@@ -65,16 +66,16 @@ public interface Climb {
6566
}
6667

6768
public interface Pivot {
68-
SmartNumber ALGAE_HOLDING_SPEED = new SmartNumber("Pivot/Roller/Target Speeds/Rollers not spinning", 1);
69-
SmartNumber ALGAE_INTAKE_SPEED = new SmartNumber("Pivot/Roller/Target Speeds/Algae Intake Speed", -1);
69+
SmartNumber ALGAE_HOLDING_SPEED = new SmartNumber("Pivot/Roller/Algae Holding Speed", 1);
70+
SmartNumber ALGAE_INTAKE_SPEED = new SmartNumber("Pivot/Roller/Algae Intake Speed", -1);
7071

71-
SmartNumber ALGAE_SHOOT_SPEED = new SmartNumber("Pivot/Roller/Target Speeds/Algae Shoot Speed", 1);
72-
SmartNumber CORAL_SHOOT_SPEED = new SmartNumber("Pivot/Roller/Target Speeds/Rollers not spinning", -0.25);
73-
SmartNumber ROLLER_STOP_SPEED = new SmartNumber("Pivot Roller Stop Speed", 0);
74-
SmartNumber ROLLER_ROTISSERIE_SPEED = new SmartNumber("Roller Coral Hold Speed", 0.08);
72+
SmartNumber ALGAE_SHOOT_SPEED = new SmartNumber("Pivot/Roller/Algae Shoot Speed", 1);
73+
SmartNumber CORAL_SHOOT_SPEED = new SmartNumber("Pivot/Roller/Coral Shoot Speed", -0.25);
74+
SmartNumber ROLLER_STOP_SPEED = new SmartNumber("Pivot/Roller/Stop Speed", 0);
75+
SmartNumber ROLLER_ROTISSERIE_SPEED = new SmartNumber("Pivot/Roller/Coral Hold Speed", 0.08);
7576

76-
SmartNumber PIVOT_RAISE_SPEED = new SmartNumber("Pivot Raise Speed", 0.09);
77-
SmartNumber PIVOT_LOWER_SPEED = new SmartNumber("Pivot Lower Speed", -0.09);
77+
SmartNumber PIVOT_RAISE_SPEED = new SmartNumber("Pivot/Raise Speed", 0.09);
78+
SmartNumber PIVOT_LOWER_SPEED = new SmartNumber("Pivot/Lower Speed", -0.09);
7879

7980
public static final int PIVOT_MOTOR_CURRENT_LIMIT = 60;
8081
public static final int PIVOT_ROLLER_MOTOR_CURRENT_LIMIT = 60;
@@ -84,6 +85,9 @@ public interface Pivot {
8485

8586
public static final double PIVOT_STALL_CURRENT = 1; // Replace with actual voltage spike when stalled.
8687
public static final double PIVOT_STALL_DEBOUNCE = .25; // Placeholder
88+
89+
public static final String CTRLMODE_MANUAL = "MANUAL";
90+
public static final String CTRLMODE_STATES = "STATES";
8791

8892
Rotation2d DEFAULT_ANGLE = Rotation2d.fromDegrees(0);
8993
Rotation2d CORAL_STOW_ANGLE = Rotation2d.fromDegrees(-3);

‎src/main/java/com/stuypulse/robot/subsystems/pivot/Pivot.java‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,11 @@ public static Pivot getInstance() {
2020
}
2121

2222
protected PivotState pivotState;
23+
protected PivotControlMode pivotControlMode;
2324

2425
protected Pivot() {
2526
this.pivotState = PivotState.STOW_CORAL;
27+
this.pivotControlMode = PivotControlMode.MANUAL;
2628
}
2729

2830
public enum PivotState {
@@ -46,6 +48,21 @@ public Rotation2d getTargetAngle() {
4648
}
4749
}
4850

51+
public enum PivotControlMode {
52+
MANUAL(Settings.Pivot.CTRLMODE_MANUAL),
53+
USING_STATES(Settings.Pivot.CTRLMODE_STATES);
54+
55+
String controlMode;
56+
57+
private PivotControlMode(String controlMode) {
58+
this.controlMode = controlMode;
59+
}
60+
61+
public String getPivotControlMode() {
62+
return this.controlMode;
63+
}
64+
}
65+
4966
public abstract void setPivotState(PivotState pivotState);
5067

5168
public abstract PivotState getPivotState();
@@ -56,7 +73,7 @@ public Rotation2d getTargetAngle() {
5673

5774
public abstract void ResetPivotEncoder();
5875

59-
public abstract void SetPivotStateMode(boolean SetPivotStateMode);
76+
public abstract void SetPivotControlMode(PivotControlMode SetPivotStateMode);
6077

6178

6279
@Override

‎src/main/java/com/stuypulse/robot/subsystems/pivot/PivotImpl.java‎

Lines changed: 8 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -30,14 +30,9 @@ public class PivotImpl extends Pivot {
3030
private SparkMax rollerMotor;
3131

3232
private BStream stallDetector;
33-
34-
public boolean PivotStateModeEnabled;
35-
36-
// double CurrentRollerSetSpeed;
37-
// double CurrentPivotSetSpeed;
3833

39-
SmartNumber CurrentRollerSetSpeed = new SmartNumber("CurrentRollerSetSpeed", 0);
40-
SmartNumber CurrentPivotSetSpeed = new SmartNumber("CurrentPivotSetSpeed", 0);
34+
private SmartNumber CurrentRollerSetSpeed = new SmartNumber("CurrentRollerSetSpeed", 0);
35+
private SmartNumber CurrentPivotSetSpeed = new SmartNumber("CurrentPivotSetSpeed", 0);
4136

4237
public PivotImpl() {
4338
super();
@@ -105,8 +100,8 @@ public PivotState getPivotState() {
105100
}
106101

107102
@Override
108-
public void SetPivotStateMode(boolean SetPivotStateMode) {
109-
PivotStateModeEnabled = SetPivotStateMode;
103+
public void SetPivotControlMode(PivotControlMode pivotControlMode) {
104+
this.pivotControlMode = pivotControlMode;
110105
}
111106

112107
@Override
@@ -117,13 +112,14 @@ public void periodic() {
117112
// Maybe make this a state? Pivot stalled?
118113
setPivotMotor(0);
119114
}
120-
if (PivotStateModeEnabled) {
121-
pivotMotor.setVoltage(controller.update(pivotState.targetAngle.getDegrees(), getPivotRotation().getDegrees()));
115+
116+
if (pivotControlMode == PivotControlMode.MANUAL) {
117+
pivotMotor.setVoltage(controller.update(pivotState.targetAngle.getDegrees(), getPivotRotation().getDegrees()));
122118
}
123119

124120
SmartDashboard.putNumber("Pivot/Number of Rotations", getPivotRotation().getRotations());
125121
SmartDashboard.putNumber("Pivot/Current Angle", getPivotRotation().getDegrees());
126122
SmartDashboard.putNumber("Pivot/Supply Current", pivotMotor.getOutputCurrent());
127-
SmartDashboard.putBoolean("pivot state mode", PivotStateModeEnabled);
123+
SmartDashboard.putString("Pivot/Control mode", pivotControlMode.getPivotControlMode());
128124
}
129125
}

0 commit comments

Comments
 (0)