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 6 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
2 changes: 0 additions & 2 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 @@ -82,7 +82,6 @@ public void autonomousInit() {


robot.arm.setCoast(false, false);
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());
Expand Down Expand Up @@ -113,7 +112,6 @@ public void teleopInit() {
SmartDashboard.putString("Match State", state.name());

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());
Expand Down
4 changes: 0 additions & 4 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,10 +213,6 @@ private void configureOperatorBindings() {
operator.getBottomButton()
.onTrue(new ManagerSetGamePiece(GamePiece.CONE_TIP_OUT));

operator.getRightBumper()
.onTrue(arm.runOnce(arm::enableLimp))
.onFalse(arm.runOnce(arm::disableLimp));

// arm to neutral
operator.getDPadRight().onTrue(new IntakeAcquire())
.onFalse(new IntakeStop());
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/com/stuypulse/robot/commands/arm/routines/ArmRoutine.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,6 @@ public void execute() {
var targetState = trajectory.getStates().get(currentIndex);
arm.setTargetState(targetState);

arm.setLimp(targetState.isWristLimp(), false);

double currentShoulderTolerance = (targetState.getShoulderTolerance().orElse(shoulderTolerance)).doubleValue();
double currentWristTolerance = (targetState.getWristTolerance().orElse(wristTolerance)).doubleValue();

Expand Down
178 changes: 72 additions & 106 deletions src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,12 @@
import com.stuypulse.stuylib.control.feedback.PIDController;
import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
import com.stuypulse.stuylib.math.Angle;
import com.stuypulse.stuylib.network.SmartBoolean;
import com.stuypulse.stuylib.network.SmartNumber;
import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile;
import com.stuypulse.stuylib.streams.booleans.BStream;
import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
import com.stuypulse.stuylib.streams.filters.MotionProfile;

import java.util.Optional;

import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.constants.Settings.Arm.Shoulder;
import com.stuypulse.robot.constants.Settings.Arm.Wrist;
Expand All @@ -38,17 +37,15 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import java.util.Optional;


/**
* Double jointed arm controlled by two motion profiled PID controllers.
*
* Available control "modes":
* - setpoint control (PID+FF controllers are used) (shoulder is not allowed above maximum shoulder angle)
* - limp mode (controller output is overriden to be zero)
* - voltage override ("force" feeds a voltage to the motor)
*/
* Double jointed arm controlled by two motion profiled PID controllers.
*
* Available control "modes":
* - setpoint control (PID+FF controllers are used) (shoulder is not allowed
* above maximum shoulder angle)
* - limp mode (controller output is overriden to be zero)
* - voltage override ("force" feeds a voltage to the motor)
*/
public abstract class Arm extends SubsystemBase {

// Singleton
Expand All @@ -71,23 +68,17 @@ public static Arm getInstance() {
private final SmartNumber shoulderTargetDegrees;
private final SmartNumber wristTargetDegrees;

// Voltage overrides (used when present)
private Optional<Double> wristVoltageOverride;
private Optional<Double> shoulderVoltageOverride;

// controllers for each joint
private final Controller shoulderController;
private final AngleController wristController;

// Mechanism2d visualizer
private final ArmVisualizer armVisualizer;

// Limp mode (forces a joint to receive zero voltage)
private SmartBoolean wristLimp;
private SmartBoolean shoulderLimp;

// Voltage overrides (used when present)
private Optional<Double> wristVoltageOverride;
private Optional<Double> shoulderVoltageOverride;

private BStream wristEnabled;

private SmartNumber shoulderVelocityFeedbackDebounce;
private SmartNumber shoulderVelocityFeedbackCutoff;

Expand Down Expand Up @@ -122,78 +113,67 @@ public double doubleValue() {

@Override
public float floatValue() {
return (float)doubleValue();
return (float) doubleValue();
}

@Override
public int intValue() {
return (int)doubleValue();
return (int) doubleValue();
}

@Override
public long longValue() {
return (long)doubleValue();
return (long) doubleValue();
}
}

protected Arm() {
// These are the setpoints for the joints relative to the "horizontal" (like the
// unit circle) -- keep both
shoulderTargetDegrees = new SmartNumber("Arm/Shoulder/Target Angle (deg)", -90);
wristTargetDegrees = new SmartNumber("Arm/Wrist/Target Angle (deg)", +90);

// These numbers are used for disabling/enabling wrist control while the
// shoulder is moving -- they are no longer necessary. Remove them
shoulderVelocityFeedbackDebounce = new SmartNumber(
"Arm/Wrist/Feedback Enabled Debounce",
Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());
"Arm/Wrist/Feedback Enabled Debounce",
Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE.doubleValue());

shoulderVelocityFeedbackCutoff = new SmartNumber(
"Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)",
Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());

wristEnabled = BStream.create(this::isWristFeedbackEnabled)
.filtered(new BDebounce.Both(shoulderVelocityFeedbackDebounce));
"Arm/Wrist/Shoulder Velocity Feedback Cutoff (deg per s)",
Wrist.TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF.doubleValue());

shoulderMaxVelocity = new SmartNumber(
"Arm/Shoulder/Max Velocity",
Shoulder.TELEOP_MAX_VELOCITY.doubleValue());
"Arm/Shoulder/Max Velocity",
Shoulder.TELEOP_MAX_VELOCITY.doubleValue());
shoulderMaxAcceleration = new SmartNumber(
"Arm/Shoulder/Max Acceleration",
Shoulder.TELEOP_MAX_ACCELERATION.doubleValue());
"Arm/Shoulder/Max Acceleration",
Shoulder.TELEOP_MAX_ACCELERATION.doubleValue());

wristMaxVelocity = new SmartNumber(
"Arm/Wrist/Max Velocity",
Wrist.TELEOP_MAX_VELOCITY.doubleValue());
"Arm/Wrist/Max Velocity",
Wrist.TELEOP_MAX_VELOCITY.doubleValue());
wristMaxAcceleration = new SmartNumber(
"Arm/Wrist/Max Acceleration",
Wrist.TELEOP_MAX_ACCELERATION.doubleValue());

shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, Shoulder.Feedforward.kA).position()
.add(new ArmEncoderFeedforward(new GamePiecekG()))
.add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs))
.add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD))
.setSetpointFilter(
new MotionProfile(
shoulderMaxVelocity.filtered(Math::toRadians).number(),
shoulderMaxAcceleration.filtered(Math::toRadians).number()))
.setOutputFilter(x -> {
if (isShoulderLimp()) return 0;
return shoulderVoltageOverride.orElse(x);
})
;
"Arm/Wrist/Max Acceleration",
Wrist.TELEOP_MAX_ACCELERATION.doubleValue());

shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV,
Shoulder.Feedforward.kA).position()
.add(new ArmEncoderFeedforward(new GamePiecekG()))
.add(new ArmDriveFeedforward(new GamePiecekG(), SwerveDrive.getInstance()::getForwardAccelerationGs))
.add(new PIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD))
.setSetpointFilter(
new MotionProfile(
shoulderMaxVelocity.filtered(Math::toRadians).number(),
shoulderMaxAcceleration.filtered(Math::toRadians).number()));

wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle()
.add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG))
.add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)
.setOutputFilter(x -> wristEnabled.get() ? x : 0))
.setSetpointFilter(
new AMotionProfile(
wristMaxVelocity.filtered(Math::toRadians).number(),
wristMaxAcceleration.filtered(Math::toRadians).number()))
.setOutputFilter(x -> {
if (isWristLimp()) return 0;
return wristVoltageOverride.orElse(x);
});

wristLimp = new SmartBoolean("Arm/Wrist/Is Limp?", false);
shoulderLimp = new SmartBoolean("Arm/Shoulder/Is Limp?", false);
.add(new ArmEncoderAngleFeedforward(Wrist.Feedforward.kG))
.add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)
.setSetpointFilter(
new AMotionProfile(
wristMaxVelocity.filtered(Math::toRadians).number(),
wristMaxAcceleration.filtered(Math::toRadians).number())));

wristVoltageOverride = Optional.empty();
shoulderVoltageOverride = Optional.empty();
Expand All @@ -214,17 +194,9 @@ public void disableGamePieceGravityCompensation() {
}

// Arm Control Overrides

private final boolean isWristLimp() {
return wristLimp.get();
}

private final boolean isShoulderLimp() {
return shoulderLimp.get();
}

private final boolean isWristFeedbackEnabled() {
return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units.degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue());
return Math.abs(getShoulderVelocityRadiansPerSecond()) < Units
.degreesToRadians(shoulderVelocityFeedbackCutoff.doubleValue());
}

// Set kinematic constraints
Expand All @@ -239,7 +211,6 @@ public final void setWristConstraints(Number velocity, Number acceleration) {
wristMaxAcceleration.set(acceleration);
}


// Read target State
public final Rotation2d getShoulderTargetAngle() {
return Rotation2d.fromDegrees(shoulderTargetDegrees.get());
Expand Down Expand Up @@ -305,6 +276,7 @@ public final boolean isAtTargetState(double shoulderEpsilonDegrees, double wrist

// Read angle measurements
public abstract Rotation2d getShoulderAngle();

protected abstract Rotation2d getRelativeWristAngle();

public final Rotation2d getWristAngle() {
Expand All @@ -316,6 +288,7 @@ public final ArmState getState() {
}

public abstract double getShoulderVelocityRadiansPerSecond();

public abstract double getWristVelocityRadiansPerSecond();

// Set a voltage override
Expand All @@ -329,34 +302,23 @@ public void setWristVoltage(double voltage) {

// Feed a voltage to the hardware layer
protected abstract void setShoulderVoltageImpl(double voltage);

protected abstract void setWristVoltageImpl(double voltage);

// set coast / brake mode
public void setCoast(boolean wristCoast, boolean shoulderCoast) {}

// set if the ligaments are "limp" (zero voltage)
public final void setLimp(boolean wristLimp, boolean shoulderLimp) {
this.wristLimp.set(wristLimp);
this.shoulderLimp.set(shoulderLimp);
}

public final void enableLimp() {
setLimp(true, true);
}
public final void disableLimp() {
setLimp(false, false);
}

// Arm Visualizer
public final ArmVisualizer getVisualizer() {
return armVisualizer;
}


@Override
public final void periodic() {
// Validate shoulder and wrist target states
Rotation2d shoulderTarget = getShoulderTargetAngle();
Rotation2d wristTarget = getWristTargetAngle();
// Rotation2d wristTarget = getWristTargetAngle();

double normalizedDeg = shoulderTarget.minus(Rotation2d.fromDegrees(-90)).getDegrees();

Expand All @@ -366,46 +328,50 @@ public final void periodic() {
setShoulderTargetAngle(Rotation2d.fromDegrees(180 - Shoulder.MAX_SHOULDER_ANGLE.get()));
}


// Run control loops on validated target angles
shoulderController.update(
getWrappedShoulderAngle(getShoulderTargetAngle()),
getWrappedShoulderAngle(getShoulderAngle()));
getWrappedShoulderAngle(getShoulderTargetAngle()),
getWrappedShoulderAngle(getShoulderAngle()));

SmartDashboard.putNumber("Arm/Shoulder/Wrapped Angle", getWrappedShoulderAngle(getShoulderAngle()));
SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle", getWrappedShoulderAngle(getShoulderTargetAngle()));
SmartDashboard.putNumber("Arm/Shoulder/Wrapped Target Angle",
getWrappedShoulderAngle(getShoulderTargetAngle()));

wristController.update(
Angle.fromRotation2d(getWristTargetAngle()),
Angle.fromRotation2d(getWristAngle()));
Angle.fromRotation2d(getWristTargetAngle()),
Angle.fromRotation2d(getWristAngle()));

setWristVoltageImpl(wristController.getOutput());
setShoulderVoltageImpl(shoulderController.getOutput());

armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()), wristController.getSetpoint().toDegrees());
armVisualizer.setTargetAngles(Units.radiansToDegrees(shoulderController.getSetpoint()),
wristController.getSetpoint().toDegrees());
armVisualizer.setMeasuredAngles(getShoulderAngle().getDegrees(), getWristAngle().getDegrees());
armVisualizer.setFieldArm(Odometry.getInstance().getPose(), getState());

SmartDashboard.putNumber("Arm/Shoulder/Angle (deg)", getShoulderAngle().getDegrees());
SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)", Units.radiansToDegrees(shoulderController.getSetpoint()));
SmartDashboard.putNumber("Arm/Shoulder/Setpoint (deg)",
Units.radiansToDegrees(shoulderController.getSetpoint()));
SmartDashboard.putNumber("Arm/Shoulder/Error (deg)", Units.radiansToDegrees(shoulderController.getError()));
SmartDashboard.putNumber("Arm/Shoulder/Output (V)", shoulderController.getOutput());
SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)", Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond()));
SmartDashboard.putNumber("Arm/Shoulder/Velocity (deg per s)",
Units.radiansToDegrees(getShoulderVelocityRadiansPerSecond()));

SmartDashboard.putNumber("Arm/Wrist/Angle (deg)", getWristAngle().getDegrees());
SmartDashboard.putNumber("Arm/Wrist/Relative Angle (deg)", getRelativeWristAngle().getDegrees());
SmartDashboard.putNumber("Arm/Wrist/Setpoint (deg)", wristController.getSetpoint().toDegrees());
SmartDashboard.putNumber("Arm/Wrist/Error (deg)", wristController.getError().toDegrees());
SmartDashboard.putNumber("Arm/Wrist/Output (V)", wristController.getOutput());
SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)", Units.radiansToDegrees(getWristVelocityRadiansPerSecond()));
SmartDashboard.putNumber("Arm/Wrist/Velocity (deg per s)",
Units.radiansToDegrees(getWristVelocityRadiansPerSecond()));
SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled Raw", isWristFeedbackEnabled());
SmartDashboard.putBoolean("Arm/Wrist/Feedback Enabled", wristEnabled.get());
SmartDashboard.putNumber("Arm/Shoulder/kG", new GamePiecekG().doubleValue());

SmartDashboard.putBoolean("Arm/Shoulder/Game Piece Compensation", pieceGravityCompensation);

periodicallyCalled();
}

public void periodicallyCalled() {}
public void periodicallyCalled() {
}
}
Loading