Skip to content

Commit 998ce0c

Browse files
k4limulk4limul
committed
Add intermediate state logic and ArmSetState
1 parent d607571 commit 998ce0c

File tree

2 files changed

+81
-34
lines changed

2 files changed

+81
-34
lines changed

‎src/main/java/com/stuypulse/robot/commands/ArmSetState.java

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
package com.stuypulse.robot.commands;
2+
3+
import com.stuypulse.robot.subsystems.double_jointed_arm.DoubleJointedArm;
4+
import com.stuypulse.robot.subsystems.double_jointed_arm.DoubleJointedArm.ArmState;
5+
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
8+
public class ArmSetState extends Command {
9+
private DoubleJointedArm arm;
10+
private ArmState state;
11+
12+
public ArmSetState(ArmState state) {
13+
this.state = state;
14+
this.arm = DoubleJointedArm.getInstance();
15+
addRequirements(arm);
16+
}
17+
18+
@Override
19+
public void initialize() {
20+
if (arm.getState().isFront() != state.isFront()) {
21+
arm.setState(state);
22+
arm.switchSides();
23+
} else {
24+
arm.setState(state);
25+
}
26+
}
27+
28+
@Override
29+
public boolean isFinished() {
30+
return arm.atTarget() && arm.getState() == state;
31+
}
32+
}

‎src/main/java/com/stuypulse/robot/subsystems/double_jointed_arm/DoubleJointedArm.java

Lines changed: 49 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -31,14 +31,19 @@ public static DoubleJointedArm getInstance() {
3131
private DoubleJointedArmIOInputsAutoLogged inputs = new DoubleJointedArmIOInputsAutoLogged();
3232

3333
private ArmState state;
34+
private ArmState storedState;
35+
private boolean intermediate;
3436

3537
public DoubleJointedArm(DoubleJointedArmIO io) {
3638
this.io = io;
37-
this.state = ArmState.STOW;
39+
state = ArmState.STOW;
40+
storedState = ArmState.STOW;
41+
intermediate = false;
3842
}
3943

4044
public enum ArmState {
4145
STOW(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10)),
46+
INT(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90)),
4247
L4_FRONT(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10)),
4348
L4_BACK(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10)),
4449
L3_FRONT(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10)),
@@ -48,16 +53,16 @@ public enum ArmState {
4853
L1_FRONT(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10)),
4954
L1_BACK(Rotation2d.fromDegrees(10), Rotation2d.fromDegrees(10));
5055

51-
private Rotation2d shoulderAngle;
52-
private Rotation2d elbowAngle;
53-
5456
private ArmState(Rotation2d shoulderAngle, Rotation2d elbowAngle){
5557
this.shoulderAngle = shoulderAngle;
5658
this.elbowAngle = elbowAngle;
57-
}
59+
}
60+
61+
private Rotation2d shoulderAngle;
62+
private Rotation2d elbowAngle;
5863

59-
public Rotation2d getShoulderTargetAngle() { return shoulderAngle; }
60-
public Rotation2d getElbowTargetAngle() { return elbowAngle; }
64+
public Rotation2d getShoulderTarget() { return shoulderAngle; }
65+
public Rotation2d getElbowTarget() { return elbowAngle; }
6166

6267
public ArmState getOpposite() {
6368
switch (this) {
@@ -74,43 +79,53 @@ public ArmState getOpposite() {
7479
}
7580

7681
public boolean isFront() {
77-
return this.name().endsWith("FRONT");
82+
return this.name().endsWith("FRONT") || this.equals(ArmState.STOW);
7883
}
7984
}
8085

81-
public void setState(ArmState state) { this.state = state; }
82-
public void switchSides() { state = state.getOpposite(); }
86+
public void setState(ArmState state) {
87+
this.state = state;
88+
}
89+
90+
public void switchSides() {
91+
if (!intermediate) {
92+
intermediate = true;
93+
storedState = state.getOpposite();
94+
setState(ArmState.INT);
95+
}
96+
}
8397

8498
@Override
8599
public void periodic() {
86100
io.updateInputs(inputs);
87101
Logger.processInputs("DoubleJointedArm", inputs);
88-
89102
Logger.recordOutput("DoubleJointedArm/CurrentState", state.name());
90-
Logger.recordOutput("DoubleJointedArm/ShoulderTarget", state.getShoulderTargetAngle().getDegrees());
91-
Logger.recordOutput("DoubleJointedArm/ElbowTarget", state.getElbowTargetAngle().getDegrees());
92-
Logger.recordOutput("DoubleJointedArm/AtTarget", isArmAtTarget(state.getElbowTargetAngle(), state.getShoulderTargetAngle()));
93-
94-
// PUT ALL CALLS TO CONTROL METHODS HERE
95-
Matrix<N2, N1> positions = VecBuilder.fill(
96-
Math.toRadians(inputs.shoulderAngle),
97-
Math.toRadians(inputs.elbowAngle)
98-
);
99-
100-
Matrix<N2, N1> velocities = VecBuilder.fill(
101-
Math.toRadians(inputs.shoulderAngularVel),
102-
Math.toRadians(inputs.elbowAngularVel)
103-
);
104-
105-
Matrix<N2, N1> accelerations = VecBuilder.fill(
106-
Math.toRadians(inputs.shoulderAngularAccel),
107-
Math.toRadians(inputs.elbowAngularAccel)
108-
);
103+
Logger.recordOutput("DoubleJointedArm/ShoulderTarget", state.getShoulderTarget().getDegrees());
104+
Logger.recordOutput("DoubleJointedArm/ElbowTarget", state.getElbowTarget().getDegrees());
105+
Logger.recordOutput("DoubleJointedArm/AtTarget", isArmAtTarget(state.getElbowTarget(), state.getShoulderTarget()));
106+
107+
double shoulderAngleRad = Math.toRadians(inputs.shoulderAngle);
108+
double elbowRelativeRad = Math.toRadians(inputs.elbowAngle - inputs.shoulderAngle);
109109

110+
double shoulderVelRad = Math.toRadians(inputs.shoulderAngularVel);
111+
double elbowRelativeVelRad = Math.toRadians(inputs.elbowAngularVel - inputs.shoulderAngularVel);
112+
113+
double shoulderAccelRad = Math.toRadians(inputs.shoulderAngularAccel);
114+
double elbowRelativeAccelRad = Math.toRadians(inputs.elbowAngularAccel - inputs.shoulderAngularAccel);
115+
116+
Matrix<N2, N1> positions = VecBuilder.fill(shoulderAngleRad, elbowRelativeRad);
117+
Matrix<N2, N1> velocities = VecBuilder.fill(shoulderVelRad, elbowRelativeVelRad);
118+
Matrix<N2, N1> accelerations = VecBuilder.fill(shoulderAccelRad, elbowRelativeAccelRad);
119+
110120
Matrix<N2, N1> ff = feedforwardVoltage(positions, velocities, accelerations);
121+
122+
if (intermediate && atTarget()) {
123+
setState(storedState);
124+
intermediate = false;
125+
}
111126

112-
io.controlShoulder(state.getShoulderTargetAngle(), ff.get(0, 0));
113-
io.controlElbow(state.getElbowTargetAngle(), ff.get(1, 0));
127+
io.controlShoulder(state.getShoulderTarget(), ff.get(0, 0));
128+
io.controlElbow(state.getElbowTarget(), ff.get(1, 0));
114129
}
115130

116131
/* GETTERS */
@@ -145,7 +160,7 @@ public Translation2d getEndPosition() { // Forward Kinematics
145160

146161
@AutoLogOutput
147162
public boolean atTarget() {
148-
return isArmAtTarget(state.getShoulderTargetAngle(), state.getElbowTargetAngle());
163+
return isArmAtTarget(state.getShoulderTarget(), state.getElbowTarget());
149164
}
150165

151166
@AutoLogOutput
@@ -197,7 +212,7 @@ public boolean isArmAtTarget(Rotation2d shoulderTargetAngle, Rotation2d elbowTar
197212
private final double ELBOW_GEAR_RATIO = Constants.DoubleJointedArm.Elbow.GEAR_RATIO;
198213

199214
/*
200-
* @param position The angles of the joints, (θ1, θ2). Note that θ2 is relative to the first joint, not the horizontal.
215+
* @param position The angles of the joints, (θ1, θ2). Note that θ2 is relative to the first joint, not the horizontal
201216
* @param velocity The velocities of the joints in deg/s
202217
* @param acceleration The accelerations of the joints in deg/s^2
203218
*/

0 commit comments

Comments
 (0)