Skip to content

Commit 7f8a69c

Browse files
nlaverdurenlaverdure
authored
Add FF to coral and algae wrists (#89)
* add offset to Kg FF term * add TODOs to suspect settings * calculate Kv * reduce visibility where able * Kg, Ks found empirically
1 parent 577e37e commit 7f8a69c

File tree

5 files changed

+116
-64
lines changed

5 files changed

+116
-64
lines changed

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -164,6 +164,10 @@ public void teleopInit() {
164164
climber.lockRatchet();
165165
elevator.resetPositionControllers();
166166
lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
167+
168+
// Test wrist feedforwards
169+
// algaeWrist.setDefaultCommand(algaeWrist.createJoystickControlCommand(operator.getHID()));
170+
// coralWrist.setDefaultCommand(coralWrist.createJoystickControlCommand(operator.getHID()));
167171
}
168172

169173
@Override
@@ -216,7 +220,7 @@ private void configureOperatorButtonBindings() {
216220

217221
Trigger algaeMode = operator.leftBumper();
218222

219-
// Test coral wrist motion
223+
// Test wrist motion
220224
operator.back()
221225
.onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode)
222226
.alongWith(algaeWrist.createSetAngleCommand(AlgaeWristState.Floor)));

‎src/main/java/frc/robot/elevator/AlgaeWrist.java

Lines changed: 34 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,10 @@
1313
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
1414
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
1515
import com.revrobotics.spark.config.SparkMaxConfig;
16+
import edu.wpi.first.math.MathUtil;
17+
import edu.wpi.first.math.controller.ArmFeedforward;
1618
import edu.wpi.first.math.controller.ProfiledPIDController;
19+
import edu.wpi.first.wpilibj.XboxController;
1720
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1821
import edu.wpi.first.wpilibj2.command.Command;
1922
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
@@ -28,12 +31,14 @@ public class AlgaeWrist extends SubsystemBase {
2831
private final SparkMaxConfig config = new SparkMaxConfig();
2932
private final SparkAbsoluteEncoder encoder = motor.getAbsoluteEncoder();
3033

31-
private final ProfiledPIDController controller =
34+
private final ProfiledPIDController feedback =
3235
new ProfiledPIDController(
3336
AlgaeWristConstants.kP,
3437
AlgaeWristConstants.kI,
3538
AlgaeWristConstants.kD,
3639
AlgaeWristConstants.kConstraints);
40+
private final ArmFeedforward feedforward =
41+
new ArmFeedforward(AlgaeWristConstants.kS, AlgaeWristConstants.kG, AlgaeWristConstants.kV);
3742

3843
private AlgaeWristState targetState = AlgaeWristState.Unknown;
3944

@@ -49,9 +54,9 @@ public AlgaeWrist() {
4954
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
5055

5156
config.absoluteEncoder
52-
.inverted(true)
57+
.inverted(true) // TODO: determine if has any effect
5358
.zeroCentered(true)
54-
.zeroOffset(AlgaeWristConstants.kPositionOffset.in(Rotations))
59+
.zeroOffset(AlgaeWristConstants.kZeroOffset.in(Rotations))
5560
.positionConversionFactor(AlgaeWristConstants.kPositionConversionFactor.in(Radians))
5661
.velocityConversionFactor(AlgaeWristConstants.kVelocityConversionFactor.in(RadiansPerSecond));
5762

@@ -64,10 +69,10 @@ public AlgaeWrist() {
6469

6570
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
6671

67-
controller.setTolerance(AlgaeWristConstants.kAllowableAngleError.in(Radians));
68-
controller.setIZone(AlgaeWristConstants.kIZone.in(Radians));
69-
controller.enableContinuousInput(0, 2.0 * Math.PI);
70-
// controller.setIntegratorRange();
72+
feedback.setTolerance(AlgaeWristConstants.kAllowableError.in(Radians));
73+
feedback.setIZone(AlgaeWristConstants.kIZone.in(Radians));
74+
feedback.enableContinuousInput(0, 2.0 * Math.PI); // TODO: determine if has any effect
75+
// feedback.setIntegratorRange();
7176

7277
setDefaultCommand(createRemainAtCurrentAngleCommand());
7378
}
@@ -79,31 +84,34 @@ public void periodic() {
7984
SmartDashboard.putNumber("Algae Wrist/Current Angular Velocity RPS", encoder.getVelocity());
8085
SmartDashboard.putString("Algae Wrist/Target State", getTargetState().name());
8186
SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Degrees", getSetpointAngleDegrees());
87+
SmartDashboard.putNumber("Algae Wrist/Setpoint Angle Radians", feedback.getSetpoint().position);
8288
SmartDashboard.putNumber(
83-
"Algae Wrist/Setpoint Angle Radians", controller.getSetpoint().position);
84-
SmartDashboard.putNumber(
85-
"Algae Wrist/Setpoint Angular Velocity RPS", controller.getSetpoint().velocity);
89+
"Algae Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
8690
SmartDashboard.putNumber("Algae Wrist/Applied Duty Cycle", motor.getAppliedOutput());
8791
SmartDashboard.putNumber("Algae Wrist/Current", motor.getOutputCurrent());
88-
SmartDashboard.putBoolean("Algae Wrist/At Goal", controller.atGoal());
92+
SmartDashboard.putBoolean("Algae Wrist/At Goal", feedback.atGoal());
8993
}
9094

9195
private double getCurrentAngleDegrees() {
9296
return Radians.of(encoder.getPosition()).in(Degrees);
9397
}
9498

9599
private double getSetpointAngleDegrees() {
96-
return Radians.of(controller.getSetpoint().position).in(Degrees);
100+
return Radians.of(feedback.getSetpoint().position).in(Degrees);
97101
}
98102

99103
public void resetController() {
100-
controller.reset(encoder.getPosition(), encoder.getVelocity());
104+
feedback.reset(encoder.getPosition(), encoder.getVelocity());
101105
}
102106

103107
private void control() {
108+
double currentPosition = encoder.getPosition();
109+
double offsetPosition =
110+
currentPosition + AlgaeWristConstants.kCenterOfGravityOffset.in(Radians);
111+
double currentVelocitySetpoint = feedback.getSetpoint().velocity;
104112
motor.setVoltage(
105-
controller.calculate(encoder.getPosition())
106-
+ AlgaeWristConstants.kG * Math.cos(encoder.getPosition()));
113+
feedforward.calculate(offsetPosition, currentVelocitySetpoint)
114+
+ feedback.calculate(currentPosition));
107115
}
108116

109117
public AlgaeWristState getTargetState() {
@@ -115,14 +123,14 @@ public Command createSetAngleCommand(AlgaeWristState state) {
115123
// initialize
116124
() -> {
117125
targetState = state;
118-
controller.setGoal(targetState.angle.in(Radians));
126+
feedback.setGoal(targetState.angle.in(Radians));
119127
},
120128
// execute
121129
() -> control(),
122130
// end
123131
interrupted -> {},
124132
// isFinished
125-
() -> controller.atGoal(),
133+
() -> feedback.atGoal(),
126134
// requirements
127135
this);
128136
}
@@ -131,7 +139,7 @@ public Command createRemainAtCurrentAngleCommand() {
131139
return new FunctionalCommand(
132140
// initialize
133141
() -> {
134-
if (targetState == AlgaeWristState.Unknown) controller.setGoal(encoder.getPosition());
142+
if (targetState == AlgaeWristState.Unknown) feedback.setGoal(encoder.getPosition());
135143
},
136144
// execute
137145
() -> control(),
@@ -142,4 +150,12 @@ public Command createRemainAtCurrentAngleCommand() {
142150
// requirements
143151
this);
144152
}
153+
154+
public Command createJoystickControlCommand(XboxController gamepad) {
155+
return this.run(
156+
() -> {
157+
double joystickInput = MathUtil.applyDeadband(-gamepad.getLeftY(), 0.05);
158+
motor.setVoltage(joystickInput);
159+
});
160+
}
145161
}

‎src/main/java/frc/robot/elevator/CoralWrist.java

Lines changed: 35 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,10 @@
1313
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
1414
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
1515
import com.revrobotics.spark.config.SparkMaxConfig;
16+
import edu.wpi.first.math.MathUtil;
17+
import edu.wpi.first.math.controller.ArmFeedforward;
1618
import edu.wpi.first.math.controller.ProfiledPIDController;
19+
import edu.wpi.first.wpilibj.XboxController;
1720
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1821
import edu.wpi.first.wpilibj2.command.Command;
1922
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
@@ -28,12 +31,14 @@ public class CoralWrist extends SubsystemBase {
2831
private final SparkMaxConfig config = new SparkMaxConfig();
2932
private final SparkAbsoluteEncoder encoder = motor.getAbsoluteEncoder();
3033

31-
private final ProfiledPIDController controller =
34+
private final ProfiledPIDController feedback =
3235
new ProfiledPIDController(
3336
CoralWristConstants.kP,
3437
CoralWristConstants.kI,
3538
CoralWristConstants.kD,
3639
CoralWristConstants.kConstraints);
40+
private final ArmFeedforward feedforward =
41+
new ArmFeedforward(CoralWristConstants.kS, CoralWristConstants.kG, CoralWristConstants.kV);
3742

3843
private CoralWristState targetState = CoralWristState.Unknown;
3944

@@ -49,24 +54,26 @@ public CoralWrist() {
4954
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
5055

5156
config.absoluteEncoder
52-
.inverted(false)
57+
.inverted(false) // TODO: determine if has any effect
5358
.zeroCentered(false)
54-
.zeroOffset(CoralWristConstants.kPositionOffset.in(Rotations))
59+
.zeroOffset(CoralWristConstants.kZeroOffset.in(Rotations))
5560
.positionConversionFactor(CoralWristConstants.kPositionConversionFactor.in(Radians))
5661
.velocityConversionFactor(CoralWristConstants.kVelocityConversionFactor.in(RadiansPerSecond));
5762

5863
config.softLimit
5964
.reverseSoftLimit(CoralWristState.Min.angle.in(Radians))
6065
.reverseSoftLimitEnabled(true)
66+
// .reverseSoftLimitEnabled(false)
6167
.forwardSoftLimit(CoralWristState.Max.angle.in(Radians))
6268
.forwardSoftLimitEnabled(true);
69+
// .forwardSoftLimitEnabled(false);
6370
// spotless:on
6471

6572
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
6673

67-
controller.setTolerance(CoralWristConstants.kAllowableAngleError.in(Radians));
68-
controller.setIZone(CoralWristConstants.kIZone.in(Radians));
69-
controller.enableContinuousInput(0, 2.0 * Math.PI);
74+
feedback.setTolerance(CoralWristConstants.kAllowableError.in(Radians));
75+
feedback.setIZone(CoralWristConstants.kIZone.in(Radians));
76+
feedback.enableContinuousInput(0, 2.0 * Math.PI); // TODO: determine if has any effect
7077
// controller.setIntegratorRange();
7178

7279
setDefaultCommand(createRemainAtCurrentAngleCommand());
@@ -79,31 +86,34 @@ public void periodic() {
7986
SmartDashboard.putNumber("Coral Wrist/Current Angular Velocity RPS", encoder.getVelocity());
8087
SmartDashboard.putString("Coral Wrist/Target State", getTargetState().name());
8188
SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Degrees", getSetpointAngleDegrees());
89+
SmartDashboard.putNumber("Coral Wrist/Setpoint Angle Radians", feedback.getSetpoint().position);
8290
SmartDashboard.putNumber(
83-
"Coral Wrist/Setpoint Angle Radians", controller.getSetpoint().position);
84-
SmartDashboard.putNumber(
85-
"Coral Wrist/Setpoint Angular Velocity RPS", controller.getSetpoint().velocity);
91+
"Coral Wrist/Setpoint Angular Velocity RPS", feedback.getSetpoint().velocity);
8692
SmartDashboard.putNumber("Coral Wrist/Applied Duty Cycle", motor.getAppliedOutput());
8793
SmartDashboard.putNumber("Coral Wrist/Current", motor.getOutputCurrent());
88-
SmartDashboard.putBoolean("Coral Wrist/At Goal", controller.atGoal());
94+
SmartDashboard.putBoolean("Coral Wrist/At Goal", feedback.atGoal());
8995
}
9096

9197
private double getCurrentAngleDegrees() {
9298
return Radians.of(encoder.getPosition()).in(Degrees);
9399
}
94100

95101
private double getSetpointAngleDegrees() {
96-
return Radians.of(controller.getSetpoint().position).in(Degrees);
102+
return Radians.of(feedback.getSetpoint().position).in(Degrees);
97103
}
98104

99105
public void resetController() {
100-
controller.reset(encoder.getPosition(), encoder.getVelocity());
106+
feedback.reset(encoder.getPosition(), encoder.getVelocity());
101107
}
102108

103109
public void control() {
110+
double currentPosition = encoder.getPosition();
111+
double offsetPosition =
112+
currentPosition + CoralWristConstants.kCenterOfGravityOffset.in(Radians);
113+
double currentVelocitySetpoint = feedback.getSetpoint().velocity;
104114
motor.setVoltage(
105-
controller.calculate(encoder.getPosition())
106-
+ CoralWristConstants.kG * Math.cos(encoder.getPosition()));
115+
feedforward.calculate(offsetPosition, currentVelocitySetpoint)
116+
+ feedback.calculate(currentPosition));
107117
}
108118

109119
public CoralWristState getTargetState() {
@@ -115,14 +125,14 @@ public Command createSetAngleCommand(CoralWristState state) {
115125
// initialize
116126
() -> {
117127
targetState = state;
118-
controller.setGoal(targetState.angle.in(Radians));
128+
feedback.setGoal(targetState.angle.in(Radians));
119129
},
120130
// execute
121131
() -> control(),
122132
// end
123133
interrupted -> {},
124134
// isFinished
125-
() -> controller.atGoal(),
135+
() -> feedback.atGoal(),
126136
// requirements
127137
this);
128138
}
@@ -131,7 +141,7 @@ public Command createRemainAtCurrentAngleCommand() {
131141
return new FunctionalCommand(
132142
// initialize
133143
() -> {
134-
if (targetState == CoralWristState.Unknown) controller.setGoal(encoder.getPosition());
144+
if (targetState == CoralWristState.Unknown) feedback.setGoal(encoder.getPosition());
135145
},
136146
// execute
137147
() -> control(),
@@ -142,4 +152,12 @@ public Command createRemainAtCurrentAngleCommand() {
142152
// requirements
143153
this);
144154
}
155+
156+
public Command createJoystickControlCommand(XboxController gamepad) {
157+
return this.run(
158+
() -> {
159+
double joystickInput = 2.0 * MathUtil.applyDeadband(-gamepad.getLeftY(), 0.05);
160+
motor.setVoltage(joystickInput);
161+
});
162+
}
145163
}

0 commit comments

Comments
 (0)