Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
WIP.
Browse files Browse the repository at this point in the history
  • Loading branch information
@haydenheroux
haydenheroux committed Feb 24, 2024
1 parent d662100 commit eed58ec
Show file tree
Hide file tree
Showing 9 changed files with 182 additions and 143 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,5 +44,5 @@ public enum Subsystem {
}

public static final Set<Subsystem> REAL_SUBSYSTEMS =
EnumSet.of(Subsystem.INTAKE, Subsystem.SHOOTER);
EnumSet.of(Subsystem.ARM, Subsystem.INTAKE, Subsystem.SHOOTER);
}
15 changes: 7 additions & 8 deletions src/main/java/frc/robot/RobotMechanisms.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import frc.robot.arm.ArmConstants.ElbowMotorConstants;
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
import frc.robot.arm.ArmState;
import frc.robot.intake.IntakeConstants.PivotMotorConstants;
Expand All @@ -20,7 +19,7 @@ public class RobotMechanisms {

private final Mechanism2d mechanism;

private MechanismLigament2d shoulder, elbow, intake;
private MechanismLigament2d shoulder, wrist, intake;

private final double WIDTH =
2
Expand Down Expand Up @@ -55,11 +54,12 @@ private void initializeArmMechanism() {
0,
armThickness,
new Color8Bit(Color.kOrange)));
elbow =
wrist =
shoulder.append(
new MechanismLigament2d(
"elbow",
ElbowMotorConstants.JOINT_CONSTANTS.lengthMeters(),
"wrist",
// WristMotorConstants.JOINT_CONSTANTS.lengthMeters(),
0.0,
0,
armThickness,
new Color8Bit(Color.kGreen)));
Expand Down Expand Up @@ -127,11 +127,10 @@ public Mechanism2d getMechanism() {

public void setArmState(ArmState state) {
Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position);
Rotation2d elbowRotation = Rotation2d.fromRotations(state.elbow().position);
// Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);
Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);

shoulder.setAngle(shoulderRotation);
elbow.setAngle(elbowRotation.minus(shoulderRotation));
wrist.setAngle(wristRotation.minus(shoulderRotation));
}

public void setIntakeAngle(Rotation2d angle) {
Expand Down
66 changes: 20 additions & 46 deletions src/main/java/frc/robot/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,8 @@
import edu.wpi.first.wpilibj2.command.Commands;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
import frc.lib.TwoJointedArmFeedforward;
import frc.lib.TwoJointedArmFeedforward.TwoJointedArmFeedforwardResult;
import frc.robot.RobotMechanisms;
import frc.robot.arm.ArmConstants.ElbowMotorConstants;
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
import frc.robot.arm.ElbowMotorIO.ElbowMotorIOValues;
import frc.robot.arm.WristMotorIO.WristMotorIOValues;
import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;

/** Subsystem class for the arm subsystem. */
Expand All @@ -28,29 +24,26 @@ public class Arm extends Subsystem {
/** Shoulder motor values. */
private final ShoulderMotorIOValues shoulderMotorValues = new ShoulderMotorIOValues();

/** Elbow motor. */
private final ElbowMotorIO elbowMotor;
/** Wrist motor. */
private final WristMotorIO wristMotor;

/** Elbow motor values. */
private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues();
/** Wrist motor values. */
private final WristMotorIOValues wristMotorValues = new WristMotorIOValues();

private ArmState goal, setpoint;

private final TwoJointedArmFeedforward feedforward;

/** Creates a new instance of the arm subsystem. */
private Arm() {
shoulderMotor = ArmFactory.createShoulderMotor();
elbowMotor = ArmFactory.createElbowMotor();
wristMotor = ArmFactory.createWristMotor();

shoulderMotor.configure();
wristMotor.configure();

setPosition(ArmState.STOW.withElbow(Rotation2d.fromDegrees(0)));
setPosition(ArmState.STOW);

goal = getPosition();
setpoint = getPosition();

feedforward =
new TwoJointedArmFeedforward(
ShoulderMotorConstants.JOINT_CONSTANTS, ElbowMotorConstants.JOINT_CONSTANTS);
}

/**
Expand All @@ -69,6 +62,7 @@ public static Arm getInstance() {
@Override
public void periodic() {
shoulderMotor.update(shoulderMotorValues);
wristMotor.update(wristMotorValues);

setSetpoint(getSetpoint().nextSetpoint(goal));

Expand All @@ -83,15 +77,13 @@ public void addToShuffleboard(ShuffleboardTab tab) {
"Shoulder Position (deg)",
() -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
position.addDouble(
"Elbow Position (deg)", () -> Units.rotationsToDegrees(elbowMotorValues.positionRotations));
"Wrist Position (deg)", () -> Units.rotationsToDegrees(wristMotorValues.positionRotations));

ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint");

setpoint.addDouble(
"Shoulder Setpoint (deg)",
() -> Units.rotationsToDegrees(getSetpoint().shoulder().position));
setpoint.addDouble(
"Elbow Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().elbow().position));
setpoint.addDouble(
"Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().wrist().position));
setpoint.addBoolean("At Setpoint?", this::atSetpoint);
Expand All @@ -100,16 +92,14 @@ public void addToShuffleboard(ShuffleboardTab tab) {

goal.addDouble(
"Shoulder Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().shoulder().position));
goal.addDouble(
"Elbow Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().elbow().position));
goal.addDouble(
"Wrist Setpoint (deg)", () -> Units.rotationsToDegrees(getGoal().wrist().position));
goal.addBoolean("At Goal?", this::atGoal);

ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages");

voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.appliedVolts);
voltages.addDouble("Elbow Voltage (V)", () -> elbowMotorValues.appliedVolts);
voltages.addDouble("Wrist Voltage (V)", () -> wristMotorValues.appliedVolts);

ShuffleboardLayout derivatives = Telemetry.addColumn(tab, "Derivatives");

Expand All @@ -119,21 +109,15 @@ public void addToShuffleboard(ShuffleboardTab tab) {
"Shoulder Acceleration (rpsps)",
() -> shoulderMotorValues.accelerationRotationsPerSecondPerSecond);
derivatives.addDouble(
"Elbow Velocity (rps)", () -> elbowMotorValues.velocityRotationsPerSecond);
"Wrist Velocity (rps)", () -> wristMotorValues.velocityRotationsPerSecond);
derivatives.addDouble(
"Elbow Acceleration (rpsps)",
() -> elbowMotorValues.accelerationRotationsPerSecondPerSecond);

ShuffleboardLayout feedforward = Telemetry.addColumn(tab, "Feedforward");

feedforward.addDouble("Shoulder Feedforward (V)", () -> calculateFeedforward().shoulderVolts());
feedforward.addDouble("Elbow Feedforward (V)", () -> calculateFeedforward().elbowVolts());
"Wrist Acceleration (rpsps)",
() -> wristMotorValues.accelerationRotationsPerSecondPerSecond);
}

public void setPosition(ArmState state) {
shoulderMotor.setPosition(state.shoulder().position);
elbowMotor.setPosition(state.elbow().position);
// wristMotor.setPosition(state.wrist().getRotations());
wristMotor.setPosition(state.wrist().position);
}

/**
Expand All @@ -143,12 +127,11 @@ public void setPosition(ArmState state) {
*/
public ArmState getPosition() {
shoulderMotor.update(shoulderMotorValues);
elbowMotor.update(elbowMotorValues);
wristMotor.update(wristMotorValues);

return new ArmState(
Rotation2d.fromRotations(shoulderMotorValues.positionRotations),
Rotation2d.fromRotations(elbowMotorValues.positionRotations),
Rotation2d.fromRotations(0));
Rotation2d.fromRotations(wristMotorValues.positionRotations));
}

public ArmState getGoal() {
Expand All @@ -175,19 +158,10 @@ private void setSetpoint(ArmState setpoint) {
this.setpoint = setpoint;

shoulderMotor.setSetpoint(setpoint.shoulder().position, setpoint.shoulder().velocity);
elbowMotor.setSetpoint(setpoint.elbow().position, setpoint.elbow().velocity);
// wristMotor.runSetpoint(state.wrist().getRotations());
wristMotor.setSetpoint(setpoint.wrist().position, setpoint.wrist().velocity);
}

public Command to(ArmState goal) {
return runOnce(() -> setGoal(goal)).andThen(Commands.waitUntil(this::atGoal));
}

public TwoJointedArmFeedforwardResult calculateFeedforward() {
ArmState position = getPosition();

return feedforward.calculateFeedforward(
Rotation2d.fromRotations(position.shoulder().position),
Rotation2d.fromRotations(position.elbow().position));
}
}
30 changes: 15 additions & 15 deletions src/main/java/frc/robot/arm/ArmConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ public static class ShoulderMotorConstants {
1);

/** Minimum angle of the shoulder joint. */
public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(12.5);
public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(27.5);

/** Maximum angle of the shoulder joint. */
public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90);
Expand Down Expand Up @@ -59,29 +59,29 @@ public static class ShoulderMotorConstants {
public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
}

/** Constants for the elbow motor. */
public static class ElbowMotorConstants {
/** Elbow motor CAN. */
public static final CAN CAN = new CAN(32); // TODO
/** Constants for the wrist motor. */
public static class WristMotorConstants {
/** Wrist motor CAN. */
public static final CAN CAN = new CAN(34);

/** Joint constants for the elbow joint. */
/** Joint constants for the wrist joint. */
public static final JointConstants JOINT_CONSTANTS =
new JointConstants(
Units.lbsToKilograms(13.006), // massKg
Units.inchesToMeters(16.825), // lengthMeters
Units.inchesToMeters(12.251799915), // radiusMeters
0.5713,
39.2911765,
Units.lbsToKilograms(13.006), // massKg // TODO
Units.inchesToMeters(16.825), // lengthMeters // TODO
Units.inchesToMeters(12.251799915), // radiusMeters // TODO
0.5713, // TODO
20.454545,
DCMotor.getNEO(1), // motor
1);

/** Minimum angle of the elbow joint. */
/** Minimum angle of the wrist joint. */
public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-90);

/** Maximum angle of the elbow joint. */
public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(180);
/** Maximum angle of the wrist joint. */
public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(200);

/** Tolerance of the elbow joint. */
/** Tolerance of the wrist joint. */
public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0);

/** Proportional gain in volts per rotation. */
Expand Down
11 changes: 7 additions & 4 deletions src/main/java/frc/robot/arm/ArmFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@ public class ArmFactory {
* @return a shoulder motor.
*/
public static ShoulderMotorIO createShoulderMotor() {
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
return new ShoulderMotorIOSparkMax();
// if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
// return new ShoulderMotorIOSparkMax();

return new ShoulderMotorIOSim();
}
Expand All @@ -24,7 +24,10 @@ public static ShoulderMotorIO createShoulderMotor() {
*
* @return an elbow motor.
*/
public static ElbowMotorIO createElbowMotor() {
return new ElbowMotorIOSim();
public static WristMotorIO createWristMotor() {
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
return new WristMotorIOSparkMax();

return new WristMotorIOSim();
}
}
Loading

0 comments on commit eed58ec

Please sign in to comment.