Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Arm code for new motor configuration #185

Open
wants to merge 16 commits into
base: main
Choose a base branch
from
Open

Arm code for new motor configuration #185

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
2addd6c
Started new arm code (Beak)
naowalrahman May 15, 2023
aeba684
Remove unneccessary code in Arm
naowalrahman May 17, 2023
3a3d048
Remove beak
naowalrahman May 18, 2023
820fc4d
Remove operator rightBumper
Keobkeig May 18, 2023
5152e82
bring back voltage overrides
naowalrahman May 18, 2023
2c08685
- Remove limp in arm routine
Keobkeig May 18, 2023
b622f79
add back limp
naowalrahman May 18, 2023
02015bc
fix limp
naowalrahman May 18, 2023
f963694
fix limp
naowalrahman May 18, 2023
c2e2127
replace limp
naowalrahman May 19, 2023
815a4c6
Add back limp methods, remove formatting changes
BenG49 May 19, 2023
1e628da
Initial wrist tuning
BenG49 May 19, 2023
0c60fb3
Remove feedback cutoff values
BenG49 May 22, 2023
8abd593
convert wrist to relative angles
colyic May 23, 2023
bbe4a01
Sim fixes and flip wrist safe angle by 180
BenG49 May 23, 2023
99e8d0b
Fix motion profile
May 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions src/main/java/com/stuypulse/robot/Robot.java
View file
Open in desktop
BenG49 marked this conversation as resolved.
Show resolved Hide resolved
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,6 @@ public void autonomousInit() {
robot.arm.setLimp(true, true);
robot.arm.setTargetState(robot.arm.getState()); // TODO: ArmHold in auton?
robot.arm.setShoulderConstraints(Shoulder.AUTON_MAX_VELOCITY, Shoulder.AUTON_MAX_ACCELERATION);
robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());
robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.AUTON_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());

RobotContainer.setCachedAlliance(DriverStation.getAlliance());

Expand Down Expand Up @@ -115,8 +113,6 @@ public void teleopInit() {
robot.arm.setCoast(false, false);
robot.arm.setLimp(false, false);
robot.arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION);
robot.arm.setShoulderVelocityFeedbackCutoff(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());
robot.arm.setShoulderVelocityFeedbackDebounce(Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());

new TeleopInit().schedule();

Expand Down
3 changes: 2 additions & 1 deletion src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
import com.stuypulse.robot.commands.plant.PlantEngage;
import com.stuypulse.robot.commands.swerve.*;
import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.subsystems.Manager.*;
import com.stuypulse.robot.util.ArmState;
import com.stuypulse.robot.util.ArmTrajectory;
Expand All @@ -28,7 +29,7 @@ public class OnePieceDock extends DebugSequentialCommandGroup {

private class FastStow extends ArmRoutine {
public FastStow() {
super(() -> new ArmState(-90, 90)
super(() -> new ArmState(-90, Settings.Arm.Wrist.WRIST_SAFE_ANGLE)
.setWristTolerance(360));
}

Expand Down
3 changes: 2 additions & 1 deletion src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import com.stuypulse.robot.commands.swerve.*;
import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay;
import com.stuypulse.robot.subsystems.Manager.*;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.util.ArmState;
import com.stuypulse.robot.util.ArmTrajectory;
import com.stuypulse.robot.util.DebugSequentialCommandGroup;
Expand All @@ -28,7 +29,7 @@ public class OnePieceMobilityDock extends DebugSequentialCommandGroup {

private class FastStow extends ArmRoutine {
public FastStow() {
super(() -> new ArmState(-90, 90)
super(() -> new ArmState(-90, Settings.Arm.Wrist.WRIST_SAFE_ANGLE)
.setWristTolerance(360));
}

Expand Down
17 changes: 1 addition & 16 deletions src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,10 +106,9 @@ public ArmIntakeSecond() {

@Override
protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
dest = new ArmState(
dest = ArmState.fromWristHorizontal(
-70.82,
10);
// 8.37);
double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees();
double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get();

Expand Down Expand Up @@ -233,13 +232,6 @@ public ThreePiece() {
new IntakeStop()
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(20);
arm.setShoulderVelocityFeedbackDebounce(0.0);
})
);

// intake third piece
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down Expand Up @@ -270,13 +262,6 @@ public ThreePiece() {
arm.runOnce(() -> arm.setWristVoltage(0))
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(5);
arm.setShoulderVelocityFeedbackDebounce(0.2);
})
);

// drive to grid and score third piece
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down
19 changes: 2 additions & 17 deletions src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ public ArmIntakeFirst() {

@Override
protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
dest = new ArmState(
dest = ArmState.fromWristHorizontal(
-70.82,
11);
// 8.37);
Expand Down Expand Up @@ -104,7 +104,7 @@ public ArmIntakeSecond() {

@Override
protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
dest = new ArmState(
dest = ArmState.fromWristHorizontal(
-70.82,
10);
// 8.37);
Expand Down Expand Up @@ -160,7 +160,6 @@ public ThreePieceWLow() {

// initial setup
addCommands(
new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)),
new ManagerSetNodeLevel(NodeLevel.LOW),
new ManagerSetGamePiece(GamePiece.CONE_TIP_UP),
new ManagerSetScoreSide(ScoreSide.BACK)
Expand Down Expand Up @@ -197,13 +196,6 @@ public ThreePieceWLow() {
arm.runOnce(() -> arm.setWristVoltage(0))
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(20);
arm.setShoulderVelocityFeedbackDebounce(0.0);
})
);

// drive to grid and score second piece :: TODO: make custom arm setpoint for this
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down Expand Up @@ -260,13 +252,6 @@ public ThreePieceWLow() {
arm.runOnce(() -> arm.setWristVoltage(0))
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(5);
arm.setShoulderVelocityFeedbackDebounce(0.2);
})
);

// drive to grid and score third piece
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down
17 changes: 1 addition & 16 deletions src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ public ArmIntakeSecond() {

@Override
protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
dest = new ArmState(
dest = ArmState.fromWristHorizontal(
-70.82,
10);
// 8.37);
Expand Down Expand Up @@ -152,7 +152,6 @@ public ThreePieceWire() {

// initial setup
addCommands(
new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)),
new ManagerSetNodeLevel(NodeLevel.LOW),
new ManagerSetGamePiece(GamePiece.CONE_TIP_UP),
new ManagerSetScoreSide(ScoreSide.BACK)
Expand Down Expand Up @@ -183,13 +182,6 @@ public ThreePieceWire() {
// arm.runOnce(() -> arm.setWristVoltage(0))
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(20);
arm.setShoulderVelocityFeedbackDebounce(0.0);
})
);

// drive to grid and score second piece :: TODO: make custom arm setpoint for this
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down Expand Up @@ -244,13 +236,6 @@ public ThreePieceWire() {
arm.runOnce(() -> arm.setWristVoltage(0))
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(5);
arm.setShoulderVelocityFeedbackDebounce(0.2);
})
);

// drive to grid and score third piece
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down
4 changes: 1 addition & 3 deletions src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {

private class FastStow extends ArmRoutine {
public FastStow() {
super(() -> new ArmState(-90, 90)
super(() -> new ArmState(-90, Wrist.WRIST_SAFE_ANGLE)
.setWristTolerance(360));
}

Expand Down Expand Up @@ -177,8 +177,6 @@ public TwoPieceDock() {

new LEDSet(LEDColor.RED),

arm.runOnce(() -> arm.setShoulderVelocityFeedbackCutoff(10.0)),

new ParallelCommandGroup(
new SwerveDriveFollowTrajectory(
paths.get("Score Piece"))
Expand Down
7 changes: 0 additions & 7 deletions src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,13 +100,6 @@ public TwoPieceWire() {
arm.runOnce(() -> arm.setWristVoltage(0))
);

addCommands(
arm.runOnce(() -> {
arm.setShoulderVelocityFeedbackCutoff(20);
arm.setShoulderVelocityFeedbackDebounce(0.0);
})
);

// drive to grid and score second piece
addCommands(
new ManagerSetGamePiece(GamePiece.CUBE),
Expand Down
48 changes: 24 additions & 24 deletions src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,45 +14,45 @@ public interface ArmTrajectories {
/* Intaking */

public interface Acquire {
ArmState kCone = new ArmState(
ArmState kCone = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Acquire Cone Shoulder", -75),
new SmartNumber("Arm Trajectories/Acquire Cone Wrist", 7));
ArmState kCube = new ArmState(
ArmState kCube = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Acquire Cube Shoulder", -75),
new SmartNumber("Arm Trajectories/Acquire Cube Wrist", 12));

ArmState kHPCone = new ArmState(
ArmState kHPCone = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Acquire HP Cone Shoulder", 0),
new SmartNumber("Arm Trajectories/Acquire HP Cone Wrist", 0));

ArmState kIntermediate = new ArmState(
ArmState kIntermediate = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Acquire Intermediate Front Shoulder", -55),
new SmartNumber("Arm Trajectories/Acquire Intermediate Front Wrist", 0));

ArmState kCubeAuton = new ArmState(
ArmState kCubeAuton = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Auton Acquire Cube Front Shoulder", -70.82),
new SmartNumber("Arm Trajectories/Auton Acquire Cube Front Wrist", 8.37));

ArmState kBOOMCubeAuton = new ArmState(
ArmState kBOOMCubeAuton = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Auton BOOM Acquire Cube Front Shoulder", -70.82),
new SmartNumber("Arm Trajectories/Auton BOOM Acquire Cube Front Wrist", 8.37));
ArmState kIntermediateAuton = new ArmState(
ArmState kIntermediateAuton = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Auton Acquire Intermediate Front Shoulder", -45),
new SmartNumber("Arm Trajectories/Auton Acquire Intermediate Front Wrist", 0));
}

public interface Deacquire {
ArmState kFrontTrajectory = new ArmState(
ArmState kFrontTrajectory = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Deacquire Front Shoulder", -77),
new SmartNumber("Arm Trajectories/Deacquire Front Wrist", 90));

ArmState kBackTrajectory = new ArmState(
ArmState kBackTrajectory = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Deacquire Back Shoulder", -134),
new SmartNumber("Arm Trajectories/Deacquire Back Wrist", -22.9));
}

public interface Stow {
ArmState kTrajectory = new ArmState(
ArmState kTrajectory = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Stowed Front Shoulder", -85),
new SmartNumber("Arm Trajectories/Stowed Front Wrist", 165));
}
Expand All @@ -62,69 +62,69 @@ public interface Stow {

public interface Ready {
public interface Mid {
ArmState kConeTipUpBack = new ArmState(
ArmState kConeTipUpBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Shoulder", -172),
new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist", 136));

ArmState kConeTipInBack = new ArmState(
ArmState kConeTipInBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -171),
new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -175));

// -4, 0
ArmState kConeTipOutFront = new ArmState(
ArmState kConeTipOutFront = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready Mid Tip Out/Shoulder", -14),
new SmartNumber("Arm Trajectories/Ready Mid Tip Out/Wrist", 42));

ArmState kAutonCubeBack = new ArmState(
ArmState kAutonCubeBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Shoulder", -165),
new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133));

ArmState kCubeFront = new ArmState(
ArmState kCubeFront = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -21),
new SmartNumber("Arm Trajectories/Mid Cube Front/Wrist", 49));

ArmState kCubeBack = new ArmState(
ArmState kCubeBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Mid Cube Back/Shoulder", -175),
new SmartNumber("Arm Trajectories/Mid Cube Back/Wrist", -62));
}

public interface High {
ArmState kConeTipInBack = new ArmState(
ArmState kConeTipInBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -185),
new SmartNumber("Arm Trajectories/Ready High Tip In Back Wrist", -180));

// -175, 128
ArmState kConeTipUpBack = new ArmState(
ArmState kConeTipUpBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready High Tip Up Back Shoulder", -179),
new SmartNumber("Arm Trajectories/Ready High Tip Up Back Wrist", 136));

ArmState kConeTipOutFront = new ArmState(
ArmState kConeTipOutFront = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", 3),
new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 37));

ArmState kCubeFront = new ArmState(
ArmState kCubeFront = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/High Cube Front/Shoulder", -5),
new SmartNumber("Arm Trajectories/High Cube Front/Wrist", 46));

ArmState kCubeAutonBack = new ArmState(
ArmState kCubeAutonBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/High Auton Cube Back/Shoulder", -186 - 5),
new SmartNumber("Arm Trajectories/High Auton Cube Back/Wrist", -138 + 5));

ArmState kCubeBack = new ArmState(
ArmState kCubeBack = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/High Cube Back/Shoulder", -186),
new SmartNumber("Arm Trajectories/High Cube Back/Wrist", -138));
}
}

public interface Score {
public interface High {
ArmState kConeTipOutFront = new ArmState(
ArmState kConeTipOutFront = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -5),
new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 37));
}

public interface Mid {
ArmState kConeTipOutFront = new ArmState(
ArmState kConeTipOutFront = ArmState.fromWristHorizontal(
new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Shoulder", -28 - 5),
new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Wrist", 44));
}
Expand Down
16 changes: 5 additions & 11 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -242,29 +242,23 @@ public interface Wrist {
SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0);
SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0);

SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Auton Shoulder Velocity Feedback Cutoff (deg per s)", 5.0);
SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Auton Feedback Enabled Debounce", 0.15);

SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 20.0);
SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Teleop Feedback Enabled Debounce", 0.0);

SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 80);
SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 180);

SmartNumber TOLERANCE = new SmartNumber("Arm/Wrist/Tolerance (deg)", 7.0);

SmartNumber INTAKE_VOLTAGE = new SmartNumber("Arm/Wrist/Intake Voltage", 0);

public interface PID {
SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 6.0);
SmartNumber kP = new SmartNumber("Arm/Wrist/kP", 4.0);
SmartNumber kI = new SmartNumber("Arm/Wrist/kI", 0);
SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 1);
SmartNumber kD = new SmartNumber("Arm/Wrist/kD", 0.2);
}

public interface Feedforward {
SmartNumber kS = new SmartNumber("Arm/Wrist/kS", 0.0);
SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.01);
SmartNumber kA = new SmartNumber("Arm/Wrist/kA", 0.04);
SmartNumber kG = new SmartNumber("Arm/Wrist/kG", 0.0);
SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.0);
SmartNumber kV = new SmartNumber("Arm/Wrist/kV", 1.7);
}
}
}
Expand Down
Loading