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

Commit

Permalink
refactor: Remove physics-based pivot simulations. Improve mechanisms.
Browse files Browse the repository at this point in the history
  • Loading branch information
@haydenheroux
haydenheroux committed Mar 4, 2024
1 parent a3208fc commit 69952b1
Show file tree
Hide file tree
Showing 13 changed files with 164 additions and 166 deletions.
17 changes: 13 additions & 4 deletions simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
"/FMSInfo": "FMSInfo",
"/Shuffleboard/Auto/Auto Chooser": "String Chooser",
"/Shuffleboard/Auto/SendableChooser[0]": "String Chooser",
"/Shuffleboard/Odometry/Field/Field": "Field2d",
"/Shuffleboard/Vision/Field": "Field2d",
"/SmartDashboard/Arm Mechanism": "Mechanism2d",
"/SmartDashboard/Mechanism": "Mechanism2d",
Expand All @@ -19,7 +20,7 @@
"/SmartDashboard/arm mech": "Mechanism2d"
},
"windows": {
"/Shuffleboard/Auto/Auto Chooser": {
"/SmartDashboard/Mechanism": {
"window": {
"visible": true
}
Expand All @@ -46,8 +47,7 @@
},
"Velocities": {
"open": true
},
"open": true
}
},
"Climber": {
"East": {
Expand All @@ -66,7 +66,8 @@
"Intake": {
"Roller": {
"open": true
}
},
"open": true
},
"Odometry": {
"Position": {
Expand All @@ -76,6 +77,14 @@
"open": true
}
},
"Shooter": {
"Flywheel": {
"open": true
},
"Serializer": {
"open": true
}
},
"Swerve": {
"Constants": {
"open": true
Expand Down
31 changes: 31 additions & 0 deletions src/main/java/frc/lib/InterpolatableColor.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
package frc.lib;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.util.Color;

public class InterpolatableColor {

private final Color zero, one;

public InterpolatableColor(Color zero, Color one) {
this.zero = zero;
this.one = one;
}

public Color sample(double t) {
t = MathUtil.clamp(t, 0, 1);

double r = zero.red * (1 - t) + one.red * t;
double g = zero.green * (1 - t) + one.green * t;
double b = zero.blue * (1 - t) + one.blue * t;

return new Color(r, g, b);
}

public Color sample(double x, double min, double max) {
double t = x / (max - min);

return sample(t);
}

}
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,8 @@ public static RobotContainer getInstance() {
/** Initializes subsystem telemetry. */
private void initializeTelemetry() {
if (RobotConstants.USE_TELEMETRY) {
Telemetry.initializeShuffleboards(arm, climber, intake, shooter, swerve);
Telemetry.initializeShuffleboards(arm, climber, intake, odometry, shooter, swerve);
SmartDashboard.putData("Mechanism", RobotMechanisms.getInstance().getMechanism());
}

SmartDashboard.putData(auto.getAutonomousChooser());
Expand Down
66 changes: 53 additions & 13 deletions src/main/java/frc/robot/RobotMechanisms.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,12 @@
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
import frc.robot.arm.ArmConstants.WristMotorConstants;
import frc.lib.InterpolatableColor;
import frc.robot.arm.ArmState;
import frc.robot.intake.IntakeConstants.PivotMotorConstants;
import frc.robot.intake.IntakeConstants.RollerMotorConstants;
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
import frc.robot.shooter.ShooterConstants.SerializerConstants;

/** Helper class for rendering robot mechanisms. */
public class RobotMechanisms {
Expand All @@ -20,7 +23,7 @@ public class RobotMechanisms {

private final Mechanism2d mechanism;

private MechanismLigament2d shoulder, wrist, intake;
private MechanismLigament2d shoulder, shooter, serializer, intake;

private final double WIDTH =
2
Expand All @@ -31,6 +34,12 @@ public class RobotMechanisms {

private final Translation2d ORIGIN = new Translation2d(WIDTH / 2, 0);

private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray);

private final InterpolatableColor shooterColor = new InterpolatableColor(Color.kLightGray, Color.kSalmon);
private final InterpolatableColor serializerColor = new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue);
private final InterpolatableColor intakeColor = new InterpolatableColor(Color.kLightGray, Color.kPaleGreen);

private RobotMechanisms() {
mechanism = new Mechanism2d(WIDTH, HEIGHT);

Expand All @@ -54,15 +63,24 @@ private void initializeArmMechanism() {
ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(),
0,
armThickness,
new Color8Bit(Color.kOrange)));
wrist =
DEFAULT_COLOR));
shooter =
shoulder.append(
new MechanismLigament2d(
"wrist",
WristMotorConstants.JOINT_CONSTANTS.lengthMeters(),
Units.inchesToMeters(4.321),
0,
armThickness,
DEFAULT_COLOR));

serializer =
shoulder.append(
new MechanismLigament2d(
"serializer",
Units.inchesToMeters(8.771),
0,
armThickness,
new Color8Bit(Color.kGreen)));
DEFAULT_COLOR));
}

private void initializeFramePerimeterMechanisms() {
Expand All @@ -79,7 +97,7 @@ private void initializeFramePerimeterMechanisms() {
HEIGHT,
90,
framePerimeterThickness,
new Color8Bit(Color.kLightGray)));
DEFAULT_COLOR));

mechanism
.getRoot(
Expand All @@ -92,7 +110,7 @@ private void initializeFramePerimeterMechanisms() {
HEIGHT,
90,
framePerimeterThickness,
new Color8Bit(Color.kLightGray)));
DEFAULT_COLOR));
}

private void initializeIntakeMechanism() {
Expand All @@ -110,7 +128,7 @@ private void initializeIntakeMechanism() {
PivotMotorConstants.DISTANCE,
0.0,
intakeThickness,
new Color8Bit(Color.kPink)));
DEFAULT_COLOR));
}

public static RobotMechanisms getInstance() {
Expand All @@ -125,15 +143,37 @@ public Mechanism2d getMechanism() {
return mechanism;
}

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

shoulder.setAngle(shoulderRotation);
wrist.setAngle(wristRotation.plus(Rotation2d.fromDegrees(90)));

// Offset the rendered wrist rotation so that zero degrees is perpendicular to the shoulder, rather than parallel with the shoulder
Rotation2d offsetWristRotation = wristRotation.plus(Rotation2d.fromDegrees(90));
Rotation2d shooterRotation = offsetWristRotation;
Rotation2d serializerRotation = offsetWristRotation.plus(Rotation2d.fromDegrees(180));

shooter.setAngle(shooterRotation);
serializer.setAngle(serializerRotation);
}

public void updateIntake(Rotation2d pivotAngle, double rollerVelocityRotationsPerSecond) {
Color color = intakeColor.sample(Math.abs(rollerVelocityRotationsPerSecond), 0, RollerMotorConstants.MAXIMUM_SPEED);

intake.setAngle(pivotAngle);
intake.setColor(new Color8Bit(color));
}

public void setIntakeAngle(Rotation2d angle) {
intake.setAngle(angle);
public void updateShooter(double velocityMetersPerSecond) {
Color color = shooterColor.sample(Math.abs(velocityMetersPerSecond), 0, FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED);

shooter.setColor(new Color8Bit(color));
}

public void updateSerializer(double velocityMetersPerSecond) {
Color color = serializerColor.sample(Math.abs(velocityMetersPerSecond), 0, SerializerConstants.MAXIMUM_TANGENTIAL_SPEED);

serializer.setColor(new Color8Bit(color));
}
}
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
import frc.robot.RobotMechanisms;
import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues;
import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
import frc.robot.arm.WristMotorIO.WristMotorIOValues;
Expand All @@ -17,6 +18,8 @@ public class Arm extends Subsystem {
/** Instance variable for the arm subsystem singleton. */
private static Arm instance = null;

private final RobotMechanisms mechanism;

/** Limit switch. */
private final LimitSwitchIO limitSwitch;

Expand All @@ -39,6 +42,8 @@ public class Arm extends Subsystem {

/** Creates a new instance of the arm subsystem. */
private Arm() {
mechanism = RobotMechanisms.getInstance();

limitSwitch = ArmFactory.createLimitSwitch();
shoulderMotor = ArmFactory.createShoulderMotor();
wristMotor = ArmFactory.createWristMotor();
Expand Down Expand Up @@ -76,6 +81,8 @@ public void periodic() {
shoulderMotor.update(shoulderMotorValues);
wristMotor.update(wristMotorValues);

mechanism.updateArm(getPosition());

setSetpoint(setpoint.nextSetpoint(goal));
}

Expand Down
66 changes: 12 additions & 54 deletions src/main/java/frc/robot/arm/ShoulderMotorIOSim.java
View file
Original file line number Diff line number Diff line change
@@ -1,39 +1,19 @@
package frc.robot.arm;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
import frc.lib.AccelerationCalculator;
import frc.robot.RobotConstants;
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;

/** Simulated shoulder motor. */
public class ShoulderMotorIOSim implements ShoulderMotorIO {

private final SingleJointedArmSim singleJointedArmSim;

private final PIDController feedback;

private double positionRotations;
private double velocityRotationsPerSecond;
private double inputVoltage;

private final AccelerationCalculator accelerationCalculator;

/** Creates a new simulated shoulder motor. */
public ShoulderMotorIOSim() {
singleJointedArmSim =
new SingleJointedArmSim(
ShoulderMotorConstants.JOINT_CONSTANTS.motor(),
ShoulderMotorConstants.JOINT_CONSTANTS.gearing(),
ShoulderMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(),
ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters(),
ShoulderMotorConstants.MINIMUM_ANGLE.getRadians(),
ShoulderMotorConstants.MAXIMUM_ANGLE.getRadians(),
false,
0.0);

feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0);

accelerationCalculator = new AccelerationCalculator();
}

Expand All @@ -42,51 +22,29 @@ public void configure() {}

@Override
public void update(ShoulderMotorIOValues values) {
singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION);

values.positionRotations = getPosition();
values.velocityRotationsPerSecond = getVelocity();
values.accelerationRotationsPerSecondPerSecond = getAcceleration();
values.positionRotations = this.positionRotations;
values.velocityRotationsPerSecond = this.velocityRotationsPerSecond;
values.accelerationRotationsPerSecondPerSecond = accelerationCalculator.calculate(velocityRotationsPerSecond);

values.inputVoltage = inputVoltage;
values.currentAmps = singleJointedArmSim.getCurrentDrawAmps();
}

private double getPosition() {
return Units.radiansToRotations(singleJointedArmSim.getAngleRads());
}

private double getVelocity() {
return Units.radiansToRotations(singleJointedArmSim.getVelocityRadPerSec());
}

private double getAcceleration() {
return accelerationCalculator.calculate(getVelocity());
}

@Override
public void setPosition(double positionRotations) {
singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0.0);
this.positionRotations = positionRotations;
}

@Override
public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
double measuredPositionRotations = getPosition();

double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);

double feedforwardVolts = 0.0;

setVoltage(feedbackVolts + feedforwardVolts);
if (DriverStation.isEnabled()) {
this.positionRotations = positionRotations;
this.velocityRotationsPerSecond = velocityRotationsPerSecond;
}
}

@Override
public void setVoltage(double volts) {
if (DriverStation.isEnabled()) {
inputVoltage = volts;
singleJointedArmSim.setInputVoltage(inputVoltage);
} else {
singleJointedArmSim.setInputVoltage(0.0);
}
// TODO
this.inputVoltage = volts;
}
}
Loading

0 comments on commit 69952b1

Please sign in to comment.