Skip to content

Commit f4a0bc1

Browse files
FRC1FRC1
FRC1
authored and
FRC1
committed
Added control scheme for lift, made PID controller
Made a new function in InputSubsystem that takes inputs from the 3 control schemes: - Used B and X for xbox controller - Used buttons 3 and 4 for both schemes using the joysticks - Returns the desired number of rotations to reach the next/previous height depending on the input - The number of rotations are stored in Constants.java for each height of the lift Started configuring PID controller in ArmSubsystem. Next Steps: - Complete the ArmSubsystem periodic and move the lift motors according to the desired stage - Test the lift to find desired heights (in rotations) of each stage
1 parent 3edbe81 commit f4a0bc1

File tree

4 files changed

+95
-25
lines changed

4 files changed

+95
-25
lines changed

‎README.md

+2-1
Original file line numberDiff line numberDiff line change
@@ -5,4 +5,5 @@ Code for the ESHS P.O.T.A.T.O.E.S. swerve drive test platform.
55

66
## To-do list
77
1. Temporarily disable feedForward mechanism (example code) to see what PID controller does, and vice versa. Test to see if we like how the feedForward mechanism works with the PID controller
8-
2. Socialize with multiple teams at competitions in order to learn more about robotics in general (buisness cards included)
8+
2. Socialize with multiple teams at competitions in order to learn more about robotics in general (buisness cards included)
9+
3. Measure the height of each coral branch and associate number of rotations to each height. Then set each of those values to the corresponding variables in Constants.java.

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

+11-5
Original file line numberDiff line numberDiff line change
@@ -161,11 +161,17 @@ public static class ArmConstants {
161161
// CAN IDs; this way, we can avoid any conflicts.
162162
public static final int LEFT_LIFT_CAN_ID = 20;
163163
public static final int RIGHT_LIFT_CAN_ID = 21;
164-
public static final int LEFT_ALGAE_CAN_ID = 22;
165-
public static final int RIGHT_ALGAE_CAN_ID = 23;
166-
public static final int CORAL_INTAKE_CAN_ID = 24;
167-
168-
public static final double LIFT_SPEED = 0;
164+
public static final int LEFT_CORAL_CAN_ID = 22;
165+
public static final int RIGHT_CORAL_CAN_ID = 23;
166+
public static final int ALGAE_INTAKE_CAN_ID = 24;
167+
168+
/**
169+
* The heights of each stage will be in terms of rotations, which need to updated with testing.
170+
*/
171+
public static final double DEFAULT_HEIGHT = 0;
172+
public static final double LIFT_HEIGHT_1 = 1;
173+
public static final double LIFT_HEIGHT_2 = 2;
174+
public static final double LIFT_HEIGHT_3 = 3;
169175
}
170176

171177
}

‎src/main/java/frc/robot/subsystems/ArmSubsystem.java

+31-14
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,14 @@
11
package frc.robot.subsystems;
22

3+
import com.revrobotics.RelativeEncoder;
34
import com.revrobotics.servohub.ServoHub.ResetMode;
5+
import com.revrobotics.spark.SparkClosedLoopController;
46
import com.revrobotics.spark.SparkMax;
57
import com.revrobotics.spark.SparkBase.PersistMode;
68
import com.revrobotics.spark.SparkLowLevel.MotorType;
9+
import com.revrobotics.spark.config.ClosedLoopConfig;
710
import com.revrobotics.spark.config.SparkMaxConfig;
11+
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
812
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
913

1014
import edu.wpi.first.util.sendable.SendableBuilder;
@@ -28,44 +32,57 @@ public class ArmSubsystem extends SubsystemBase {
2832
/**
2933
* Left algae intake motor.
3034
*/
31-
private SparkMax LeftAlgae;
35+
private SparkMax LeftCoral;
3236

3337
/**
3438
* Right algae intake motor. Similar to the lift motors,
3539
* this will be a follower of the left algae intake motor.
3640
*/
37-
private SparkMax RightAlgae;
41+
private SparkMax RightCoral;
3842

3943
/**
4044
* Coral intake motor
4145
*/
42-
private SparkMax CoralIntake;
46+
private SparkMax AlgaeIntake;
47+
48+
private RelativeEncoder leftLiftEncoder;
49+
50+
private SparkClosedLoopController pidController;
4351

4452
public ArmSubsystem() {
4553
LeftLift = new SparkMax(Constants.ArmConstants.LEFT_LIFT_CAN_ID, MotorType.kBrushless);
4654
RightLift = new SparkMax(Constants.ArmConstants.RIGHT_LIFT_CAN_ID, MotorType.kBrushless);
47-
LeftAlgae = new SparkMax(Constants.ArmConstants.LEFT_ALGAE_CAN_ID, MotorType.kBrushless);
48-
RightAlgae = new SparkMax(Constants.ArmConstants.RIGHT_ALGAE_CAN_ID, MotorType.kBrushless);
49-
CoralIntake = new SparkMax(Constants.ArmConstants.CORAL_INTAKE_CAN_ID, MotorType.kBrushless);
55+
LeftCoral = new SparkMax(Constants.ArmConstants.LEFT_CORAL_CAN_ID, MotorType.kBrushless);
56+
RightCoral = new SparkMax(Constants.ArmConstants.RIGHT_CORAL_CAN_ID, MotorType.kBrushless);
57+
AlgaeIntake = new SparkMax(Constants.ArmConstants.ALGAE_INTAKE_CAN_ID, MotorType.kBrushless);
58+
59+
leftLiftEncoder = LeftLift.getEncoder();
60+
61+
pidController = LeftLift.getClosedLoopController();
5062

5163
// The idle mode of the coral intake and lift motors are set to brake to prevent them from
5264
// continuing to move after the movement is halted. The algae config idle mode is set to
5365
// coast due to the motors being in control of flywheels, meaning that the flywheels will
5466
// slowly spin to a halt rather than fully stopping immediately.
5567
SparkMaxConfig liftConfig = new SparkMaxConfig();
5668
liftConfig.idleMode(IdleMode.kBrake);
69+
liftConfig.closedLoop.pid(0.1, 0, 0);
70+
liftConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
71+
5772
SparkMaxConfig algaeConfig = new SparkMaxConfig();
5873
algaeConfig.idleMode(IdleMode.kCoast);
74+
5975
SparkMaxConfig coralConfig = new SparkMaxConfig();
60-
coralConfig.idleMode(IdleMode.kBrake);
61-
liftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID);
62-
algaeConfig.follow(Constants.ArmConstants.LEFT_ALGAE_CAN_ID);
76+
coralConfig.idleMode(IdleMode.kCoast);
77+
78+
liftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID, true);
79+
coralConfig.follow(Constants.ArmConstants.LEFT_CORAL_CAN_ID, true);
6380

6481
LeftLift.configure(liftConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
6582
RightLift.configure(liftConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
66-
LeftAlgae.configure(algaeConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
67-
RightAlgae.configure(algaeConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
68-
CoralIntake.configure(coralConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
83+
LeftCoral.configure(coralConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
84+
RightCoral.configure(coralConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
85+
AlgaeIntake.configure(algaeConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
6986
}
7087

7188
@Override
@@ -75,8 +92,8 @@ public void initSendable(SendableBuilder builder) {
7592
// as they are following them. As such, we only get the speed of the
7693
// leader, the LeftMotor. The right motor will be spinning in the opposite direction though.
7794
builder.addDoubleProperty("Lift", () -> LeftLift.get(), (armSpeed) -> LeftLift.set(armSpeed));
78-
builder.addDoubleProperty("Algae", () -> LeftAlgae.get(), (intakeSpeed) -> LeftAlgae.set(intakeSpeed));
79-
builder.addDoubleProperty("CoralIntake", () -> CoralIntake.get(), (intakeSpeed) -> CoralIntake.set(intakeSpeed));
95+
builder.addDoubleProperty("Coral", () -> LeftCoral.get(), (intakeSpeed) -> LeftCoral.set(intakeSpeed));
96+
builder.addDoubleProperty("AlgaeIntake", () -> AlgaeIntake.get(), (intakeSpeed) -> AlgaeIntake.set(intakeSpeed));
8097
initSendable(builder);
8198
}
8299

‎src/main/java/frc/robot/subsystems/InputSubsystem.java

+51-5
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package frc.robot.subsystems;
22

3+
import com.revrobotics.RelativeEncoder;
4+
35
import edu.wpi.first.math.MathUtil;
46
import edu.wpi.first.math.filter.SlewRateLimiter;
57
import edu.wpi.first.util.sendable.SendableBuilder;
@@ -70,11 +72,14 @@ public class InputSubsystem extends SubsystemBase {
7072
*/
7173
private double lastCheckTimeSeconds;
7274

75+
int currentHeight;
76+
7377
public InputSubsystem() {
7478
xboxController = null;
7579
mainJoystick = null;
7680
secondaryJoystick = null;
7781
lastCheckTimeSeconds = Timer.getFPGATimestamp() - JOYSTICK_POLLING_INTERVAL_SECONDS; // Force initial check
82+
currentHeight = 0;
7883
}
7984

8085
/**
@@ -176,11 +181,52 @@ public double getTurn() {
176181
return m_rotLimiter.calculate(turn);
177182
}
178183

179-
180-
// public double getLiftSpeed() {
181-
// double liftOut = (xboxController != null && xboxController.isConnected() && xboxController.getBButton() ? Constants.ArmConstants.LIFT_SPEED : 0);
182-
// return liftOut;
183-
// }
184+
/**
185+
* A function to return the number of desired rotations of the lift motors based on user input.
186+
* - Stage 0 represents base height of the lift when idle, which is equal to the height of the trough.
187+
* - Each additional stage (up to the top) represents the next level of the coral branch.
188+
* - Lift stage 1 -> coral level 2
189+
* - Lift stage 2 -> coral level 3
190+
* - Lift stage 3 -> coral level 4
191+
*
192+
* - Each time the user presses one of the lift triggering buttons,
193+
* the lift will automatically transition between the stages.
194+
*/
195+
public double getDesiredPosition() {
196+
double[] liftHeights = {
197+
Constants.ArmConstants.DEFAULT_HEIGHT,
198+
Constants.ArmConstants.LIFT_HEIGHT_1,
199+
Constants.ArmConstants.LIFT_HEIGHT_2,
200+
Constants.ArmConstants.LIFT_HEIGHT_3};
201+
202+
203+
204+
if(xboxController != null &&xboxController.isConnected() == true) {
205+
if(xboxController.getBButtonPressed() && currentHeight <= 3) {
206+
currentHeight++;
207+
} else if(xboxController.getXButtonPressed() && currentHeight >=0) {
208+
currentHeight--;
209+
}
210+
}
211+
212+
if(mainJoystick != null && mainJoystick.isConnected() && secondaryJoystick != null && secondaryJoystick.isConnected()) {
213+
if(secondaryJoystick.getRawButtonPressed(4) && currentHeight <= 3) {
214+
currentHeight++;
215+
} else if(secondaryJoystick.getRawButtonPressed(3) && currentHeight >=0) {
216+
currentHeight--;
217+
}
218+
} else if(mainJoystick != null && mainJoystick.isConnected()) {
219+
if(mainJoystick.getRawButtonPressed(4) && currentHeight <= 3) {
220+
currentHeight++;
221+
} else if(mainJoystick.getRawButtonPressed(3) && currentHeight >=0) {
222+
currentHeight--;
223+
}
224+
}
225+
226+
227+
228+
return liftHeights[currentHeight];
229+
}
184230

185231
/**
186232
* A function called every few seconds meant to detect when xbox controllers

0 commit comments

Comments
 (0)