From 793285100cbbdf0162a6f638160997f8713f07ee Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Thu, 23 Mar 2023 16:28:02 -0700 Subject: [PATCH 1/5] added more QOL from kirby PR --- .../java/frc/team2412/robot/Controls.java | 2 - .../robot/commands/arm/SetArmCommand.java | 8 +- .../robot/commands/arm/SetFullArmCommand.java | 9 +- .../robot/subsystems/ArmSubsystem.java | 219 +++++------------- .../robot/subsystems/LEDSubsystem.java | 14 +- 5 files changed, 86 insertions(+), 166 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index c0d6ca42..e47730eb 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -68,8 +68,6 @@ public Controls(Subsystems s) { codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT); this.s = s; - // TODO: reimpliment arm + wrist presets after comp - armManualControlOn = codriveController.start(); armManualControlOff = codriveController.back(); diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetArmCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetArmCommand.java index 1708a859..30c83d5b 100644 --- a/src/main/java/frc/team2412/robot/commands/arm/SetArmCommand.java +++ b/src/main/java/frc/team2412/robot/commands/arm/SetArmCommand.java @@ -1,7 +1,5 @@ package frc.team2412.robot.commands.arm; -import static frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.PositionType.*; - import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team2412.robot.subsystems.ArmSubsystem; import frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.PositionType; @@ -10,10 +8,12 @@ public class SetArmCommand extends CommandBase { private ArmSubsystem armSubsystem; private PositionType positionType; + private double tolerance; - public SetArmCommand(ArmSubsystem armSubsystem, PositionType positionType) { + public SetArmCommand(ArmSubsystem armSubsystem, PositionType positionType, double tolerance) { this.armSubsystem = armSubsystem; this.positionType = positionType; + this.tolerance = tolerance; } @Override @@ -31,6 +31,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return armSubsystem.isArmNearGoal(); + return armSubsystem.isArmNearGoal(tolerance); } } diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java index 765bca66..fd9f0ff5 100644 --- a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java +++ b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java @@ -8,8 +8,13 @@ public class SetFullArmCommand extends SequentialCommandGroup { public SetFullArmCommand( ArmSubsystem armSubsystem, PositionType positionType, WristPosition wristPosition) { + this(armSubsystem, positionType, wristPosition, 0.1); + } + + public SetFullArmCommand( + ArmSubsystem armSubsystem, PositionType positionType, WristPosition wristPosition, double armTolerance) { addCommands( - new SetArmCommand(armSubsystem, positionType), - new SetWristCommand(armSubsystem, wristPosition)); + new SetArmCommand(armSubsystem, positionType, armTolerance), + new SetWristCommand(armSubsystem, wristPosition)); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java index 1948c9f6..1ccc23e6 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java @@ -1,6 +1,7 @@ package frc.team2412.robot.subsystems; import static frc.team2412.robot.Hardware.*; +import static frc.team2412.robot.sim.SparkMaxSimProfile.SparkMaxConstants.*; import static frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.*; import static frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.PositionType.*; @@ -23,6 +24,8 @@ import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team2412.robot.Robot; +import frc.team2412.robot.sim.PhysicsSim; +import frc.team2412.robot.sim.SparkMaxSimProfile; public class ArmSubsystem extends SubsystemBase { @@ -32,12 +35,9 @@ public static class ArmConstants { // Conversion - public static final double TICKS_TO_INCHES = 42 / 360; // Maybe unneeded? public static final float ARM_MOTOR_TO_SHOULDER_ENCODER_RATIO = 75; public static final float WRIST_MOTOR_TO_WRIST_ENCODER_RATIO = 90; public static final float ARM_ROTATIONS_TO_SHOULDER_ENCODER_RATIO = 4; - // TODO: this is still incorrect, current delta between extended and retracted is 0.1809 but - // should be 0.25 public static final double SHOULDER_ENCODER_TO_ARM_POSITION_RATIO = 4 / 1; public static final double WRIST_ENCODER_TO_WRIST_POSITION_RATIO = 24 / 1; public static final double SHOULDER_TO_ELBOW_GEAR_RATIO = 64.0 / 48.0; @@ -47,20 +47,8 @@ public static class ArmConstants { public static final double OUTER_ARM_MASS = 5.03; public static final double INTAKE_MASS = 5.57; - // Center of mass stuff - - // inches - public static final double INNER_ARM_CENTER_OF_MASS_DISTANCE_FROM_JOINT = 11.218; - public static final double OUTER_ARM_CENTER_OF_MASS_DISTANCE_FROM_JOINT = 15.712; - public static final double WRIST_CENTER_OF_MASS_DISTANCE_FROM_JOINT = 10.85; - public static final double WRIST_CENTER_OF_MASS_RADIAN_OFFSET = -0.112; // -20.15 / 360 - - public static final double INNER_ARM_LENGTH = 25; - public static final double OUTER_ARM_LENGTH = 27.25; - // PID - // TODO: Tune PID - public static final double ARM_K_P = 3; + public static final double ARM_K_P = 4; public static final double ARM_K_I = 0; public static final double ARM_K_D = 0; @@ -91,11 +79,9 @@ public static class ArmConstants { public static final float ARM_FORWARD_LIMIT = 93.8f; // motor rotations public static final float ARM_REVERSE_LIMIT = 2; - public static final float WRIST_FORWARD_LIMIT = 58; + public static final float WRIST_FORWARD_LIMIT = 72; public static final float WRIST_REVERSE_LIMIT = 2; - - public static final double ARM_POS_TOLERANCE = 0.01; - public static final double WRIST_POS_TOLERANCE = 0.01; + public static final float WRIST_ENCODER_OFFSET = 0.227f; public static final double ARM_VELOCITY_TOLERANCE = 0.2; public static final double WRIST_VELOCITY_TOLERANCE = 0.2; @@ -108,53 +94,48 @@ public static class ArmConstants { / ARM_ROTATIONS_TO_SHOULDER_ENCODER_RATIO; // arm rotations / second^2 public static final Constraints ARM_CONSTRAINTS = - new Constraints(MAX_ARM_ACCELERATION, MAX_ARM_VELOCITY); + new Constraints(MAX_ARM_VELOCITY, MAX_ARM_ACCELERATION); public static final double MAX_WRIST_VELOCITY = .5; public static final double MAX_WRIST_ACCELERATION = 0.5; public static final Constraints WRIST_CONSTRAINTS = - new Constraints(MAX_WRIST_ACCELERATION, MAX_WRIST_VELOCITY); + new Constraints(MAX_WRIST_VELOCITY, MAX_WRIST_ACCELERATION); // ENUM /* - * TODO: update/improve presets + * TODO: Prescore presets * * Find arm and wrist angles (see onenote page) * * Values are angles that correspond to specific arm positions: * Arm Angle * Retracted Wrist Angle - * Retracted Wrist Angle (cone edition) * Prescore Wrist Angle * Scoring Wrist Angle */ public static enum PositionType { - UNKNOWN_POSITION(0.212, 0.05, 0.05, 0.46, 0.46), - ARM_LOW_POSITION(0.212, 0.05, 0.05, 0.46, 0.46), - ARM_MIDDLE_POSITION(0.415, 0.05, 0.05, 0.42, 0.43), - ARM_HIGH_POSITION(0.6546, 0.05, 0.05, 0.465, 0.473), - ARM_SUBSTATION_POSITION(0.6, 0.05, 0.05, 0.56, 0.56); // ? + UNKNOWN_POSITION(0.212, 0.05, 0.46, 0.46), + ARM_LOW_POSITION(0.212, 0.05, 0.46, 0.46), + ARM_MIDDLE_POSITION(0.415, 0.05, 0.42, 0.43), + ARM_HIGH_POSITION(0.6546, 0.05, 0.465, 0.473), + ARM_SUBSTATION_POSITION(0.6, 0.05, 0.56, 0.56); // ? public final double armAngle; public final double retractedWristAngle; - public final double retractedConeWristAngle; - public final double prescoringWristAngle; public final double scoringWristAngle; PositionType( double armAngle, double retractedWristAngle, - double retractedConeWristAngle, double prescoringWristAngle, double scoringWristAngle) { this.armAngle = armAngle; - this.retractedWristAngle = retractedWristAngle; - this.retractedConeWristAngle = retractedConeWristAngle; - this.prescoringWristAngle = prescoringWristAngle; - this.scoringWristAngle = scoringWristAngle; + this.retractedWristAngle = retractedWristAngle + WRIST_ENCODER_OFFSET; + this.prescoringWristAngle = prescoringWristAngle + WRIST_ENCODER_OFFSET; + this.scoringWristAngle = scoringWristAngle + WRIST_ENCODER_OFFSET; } } } @@ -183,19 +164,19 @@ public static enum PositionType { NetworkTableInstance NTInstance; NetworkTable NTDevices; - StringPublisher shoulderPositionPublisher; - StringPublisher elbowPositionPublisher; - StringPublisher wristPositionPublisher; + DoublePublisher shoulderPositionPublisher; + DoublePublisher elbowPositionPublisher; + DoublePublisher wristPositionPublisher; DoublePublisher armPIDPublisher; DoublePublisher armFeedforwardPublisher; - StringPublisher armGoalPublisher; + DoublePublisher armGoalPublisher; StringPublisher armPositionPublisher; BooleanPublisher armManualOverridePublisher; - StringPublisher wristGoalPublisher; + DoublePublisher wristGoalPublisher; DoubleSubscriber armVoltageOverride; BooleanSubscriber armVoltageOverrideEnable; @@ -213,7 +194,7 @@ public ArmSubsystem() { elbowEncoder = armMotor1.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); wristEncoder = wristMotor.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); - wristEncoder.setZeroOffset(0.239); + wristEncoder.setZeroOffset(0); wristEncoder.setInverted(true); elbowEncoder.setInverted(true); @@ -228,7 +209,6 @@ public ArmSubsystem() { manualOverride = true; armPID.reset(getShoulderPosition()); - // wristPID.reset(getWristPosition()); // Network Tables @@ -236,35 +216,30 @@ public ArmSubsystem() { NTDevices = NTInstance.getTable("Devices"); - shoulderPositionPublisher = NTDevices.getStringTopic("Shoulder Pos").publish(); - elbowPositionPublisher = NTDevices.getStringTopic("Elbow Pos").publish(); - wristPositionPublisher = NTDevices.getStringTopic("Wrist Pos").publish(); - + armPositionPublisher = NTDevices.getStringTopic("Position").publish(); + shoulderPositionPublisher = NTDevices.getDoubleTopic("Shoulder Pos").publish(); + elbowPositionPublisher = NTDevices.getDoubleTopic("Elbow Pos").publish(); + wristPositionPublisher = NTDevices.getDoubleTopic("Wrist Pos").publish(); armPIDPublisher = NTDevices.getDoubleTopic("Arm PID").publish(); armFeedforwardPublisher = NTDevices.getDoubleTopic("Arm Feedforward").publish(); - armGoalPublisher = NTDevices.getStringTopic("Arm Goal").publish(); - armPositionPublisher = NTDevices.getStringTopic("Position").publish(); + armGoalPublisher = NTDevices.getDoubleTopic("Arm Goal").publish(); armVoltagePublisher = NTDevices.getDoubleTopic("Arm voltage").publish(); armOutputPublisher = NTDevices.getDoubleTopic("Arm output").publish(); armCurrentPublisher = NTDevices.getDoubleTopic("Arm current").publish(); armLastSetValuePublisher = NTDevices.getDoubleTopic("Arm last set value").publish(); - - wristGoalPublisher = NTDevices.getStringTopic("Wrist Goal").publish(); - + wristGoalPublisher = NTDevices.getDoubleTopic("Wrist Goal").publish(); armManualOverridePublisher = NTDevices.getBooleanTopic("Manual Override").publish(); - shoulderPositionPublisher.set(0 + " rotations | " + 0 + " degrees"); - elbowPositionPublisher.set(0 + " rotations | " + 0 + " degrees"); - wristPositionPublisher.set(0 + " rotations | " + 0 + " degrees"); - + shoulderPositionPublisher.set(0.0); + elbowPositionPublisher.set(0.0); + wristPositionPublisher.set(0.0); + armGoalPublisher.set(0.0); armPIDPublisher.set(0.0); armFeedforwardPublisher.set(0.0); - armVoltagePublisher.set(0); - armOutputPublisher.set(0); - armCurrentPublisher.set(0); - armLastSetValuePublisher.set(0); - - armGoalPublisher.set(0.0 + " rotations | " + 0.0 + " degrees"); + armVoltagePublisher.set(0.0); + armOutputPublisher.set(0.0); + armCurrentPublisher.set(0.0); + armLastSetValuePublisher.set(0.0); } // METHODS @@ -317,14 +292,11 @@ public void setWristPID(double p, double i, double d) { wristPID.setD(d); } - public void disableShoulderLimits() { - armMotor1.enableSoftLimit(SoftLimitDirection.kForward, false); - armMotor1.enableSoftLimit(SoftLimitDirection.kReverse, false); - } + public void simInit(PhysicsSim sim) { + sim.addSparkMax(armMotor1, STALL_TORQUE, FREE_SPEED_RPM); + sim.addSparkMax(armMotor2, STALL_TORQUE, FREE_SPEED_RPM); - public void enableShoulderLimits() { - armMotor1.enableSoftLimit(SoftLimitDirection.kForward, true); - armMotor1.enableSoftLimit(SoftLimitDirection.kReverse, true); + sim.addSparkMax(wristMotor, STALL_TORQUE, FREE_SPEED_RPM); } /** @@ -395,16 +367,14 @@ public double calculateArmPID() { * @return The calculated feedforward. */ public double calculateArmFeedforward() { + // The values below are from our maximum and minimum elbow positions line of best fit for feed + // forward values // -0.111639604 0.064541504 - double wristPosition = getWristPosition(); + double elbowPosition = getElbowPosition(); - return ARM_K_S * Math.signum(armGoal - wristPosition) + wristPosition <= 0.48 + return ARM_K_S * Math.signum(armGoal - elbowPosition) + elbowPosition <= 0.48 ? 0.0 - : (wristPosition * -0.111639604 + 0.064541504); - /** - * return ARM_K_S * Math.signum(armGoal - getShoulderPosition()) + ARM_K_G * - * getAngleTowardsCenterOfMass() + ARM_K_V * 0 + ARM_K_A * 0; - */ + : (elbowPosition * -0.111639604 + 0.064541504); } /** @@ -453,15 +423,6 @@ public double getWristPosition() { return wristEncoder.getPosition(); } - /** - * Returns whether or not arm/wrist manual override is on. - * - * @return If manual override is on. - */ - public boolean getManualOverride() { - return manualOverride; - } - /** * Converts the percentOutput into volts * @@ -472,66 +433,21 @@ public double convertToVolts(double percentOutput) { return percentOutput * Robot.getInstance().getVoltage(); } + public double rotationsToRadians(double rotations) { + return rotations * 2 * Math.PI; + } + /** - * Calculates the angle towards center of mass by averaging and weighing the individual arm parts - * (inner arm, outer arm, and intake) together. + * Returns whether or not arm/wrist manual override is on. * - * @return The calculated angle in radians. + * @return If manual override is on. */ - public double getAngleTowardsCenterOfMass() { - // Theta to be in Radians, encoder values in rotations - // Inner Arm angle and Wrist Angle's 0 positions are at 180 degrees(relative to elbow angle) - // (refer to feedforward page), - // calculations directly below were made accordingly. - double innerArmTheta = Math.PI - rotationsToRadians(getShoulderPosition()); - double outerArmTheta = rotationsToRadians(getElbowPosition()); - double wristTheta = - Math.PI - (rotationsToRadians(getWristPosition()) + WRIST_CENTER_OF_MASS_RADIAN_OFFSET); - - // Calculate CoM Coordinates (relative to shoulder joint) - double innerArmCenterOfMassX = - INNER_ARM_CENTER_OF_MASS_DISTANCE_FROM_JOINT * Math.cos(innerArmTheta); - double innerArmCenterOfMassY = - INNER_ARM_CENTER_OF_MASS_DISTANCE_FROM_JOINT * Math.sin(innerArmTheta); - - double outerArmCenterOfMassX = - OUTER_ARM_CENTER_OF_MASS_DISTANCE_FROM_JOINT * Math.cos(outerArmTheta) - + INNER_ARM_LENGTH * Math.cos(innerArmTheta); - double outerArmCenterOfMassY = - OUTER_ARM_CENTER_OF_MASS_DISTANCE_FROM_JOINT * Math.sin(outerArmTheta) - + INNER_ARM_LENGTH * Math.sin(innerArmTheta); - - double wristCenterOfMassX = - WRIST_CENTER_OF_MASS_DISTANCE_FROM_JOINT * Math.cos(wristTheta) - + OUTER_ARM_LENGTH * Math.cos(outerArmTheta) - + INNER_ARM_LENGTH * Math.cos(innerArmTheta); - double wristCenterOfMassY = - WRIST_CENTER_OF_MASS_DISTANCE_FROM_JOINT * Math.sin(wristTheta) - + OUTER_ARM_LENGTH * Math.sin(outerArmTheta) - + INNER_ARM_LENGTH * Math.sin(innerArmTheta); - - // Calculate Average CoM Coordinates - double CenterOfMassX = - ((INNER_ARM_MASS * innerArmCenterOfMassX) - + (OUTER_ARM_MASS * outerArmCenterOfMassX) - + (INTAKE_MASS * wristCenterOfMassX)) - / (INNER_ARM_MASS + OUTER_ARM_MASS + INTAKE_MASS); - double CenterOfMassY = - ((INNER_ARM_MASS * innerArmCenterOfMassY) - + (OUTER_ARM_MASS * outerArmCenterOfMassY) - + (INTAKE_MASS * wristCenterOfMassY)) - / (INNER_ARM_MASS + OUTER_ARM_MASS + INTAKE_MASS); - - // Return Angle Towards CoM - return Math.atan(CenterOfMassY / CenterOfMassX); - } - - public double rotationsToRadians(double rotations) { - return rotations * 2 * Math.PI; + public boolean getManualOverride() { + return manualOverride; } - public boolean isArmNearGoal() { - return Math.abs(getElbowPosition() - armPID.getGoal().position) <= 0.2; + public boolean isArmNearGoal(double nearValue) { + return Math.abs(getElbowPosition() - armPID.getGoal().position) <= nearValue; } /** Updates the arm motor's output based off of the wrist's goal */ @@ -561,23 +477,12 @@ public void periodic() { // Network Tables - shoulderPositionPublisher.set( - getShoulderPosition() + " rotations | " + (getShoulderPosition() * 360) + " degrees"); - elbowPositionPublisher.set( - getElbowPosition() + " rotations | " + (getElbowPosition() * 360) + " degrees"); - wristPositionPublisher.set( - getWristPosition() + " rotations | " + (getWristPosition() * 360) + " degrees"); - - armGoalPublisher.set( - armPID.getGoal().position - + " rotations | " - + (armPID.getGoal().position * 360) - + " degrees"); - wristGoalPublisher.set( - (wristGoal / WRIST_MOTOR_TO_WRIST_ENCODER_RATIO) - + " encoder | " - + (wristGoal) - + " motor rotations"); + shoulderPositionPublisher.set(getShoulderPosition()); + elbowPositionPublisher.set(getElbowPosition()); + wristPositionPublisher.set(getWristPosition()); + + armGoalPublisher.set(armPID.getGoal().position); + wristGoalPublisher.set(wristGoal / WRIST_MOTOR_TO_WRIST_ENCODER_RATIO); armPIDPublisher.set(calculateArmPID()); diff --git a/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java index 1a44d501..24729c65 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LEDSubsystem.java @@ -7,15 +7,27 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDSubsystem extends SubsystemBase { + private static final double CUBE_LED_COLOR = 0.69; + private static final double CONE_LED_COLOR = 0.91; + private static final double RED_LED_COLOR = 0.61; + private final PWM blinkin; public LEDSubsystem() { blinkin = new PWM(BLINKIN_LED); - blinkin.setSpeed(0.0); // sets to red + blinkin.setSpeed(RED_LED_COLOR); // sets to red } public void setLED(double color) { blinkin.setSpeed(color); System.out.println("set color " + color); } + + public void setLEDCube() { + blinkin.setSpeed(CUBE_LED_COLOR); + } + + public void setLEDCone() { + blinkin.setSpeed(CONE_LED_COLOR); + } } From 3e2073429d24006723f4524b3a1fe1ff1d87e8e5 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Thu, 23 Mar 2023 16:31:38 -0700 Subject: [PATCH 2/5] spotless --- .../team2412/robot/commands/arm/SetFullArmCommand.java | 9 ++++++--- .../java/frc/team2412/robot/subsystems/ArmSubsystem.java | 2 -- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java index fd9f0ff5..c42aa77a 100644 --- a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java +++ b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java @@ -12,9 +12,12 @@ public SetFullArmCommand( } public SetFullArmCommand( - ArmSubsystem armSubsystem, PositionType positionType, WristPosition wristPosition, double armTolerance) { + ArmSubsystem armSubsystem, + PositionType positionType, + WristPosition wristPosition, + double armTolerance) { addCommands( - new SetArmCommand(armSubsystem, positionType, armTolerance), - new SetWristCommand(armSubsystem, wristPosition)); + new SetArmCommand(armSubsystem, positionType, armTolerance), + new SetWristCommand(armSubsystem, wristPosition)); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java index 1ccc23e6..9a4cf871 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java @@ -6,7 +6,6 @@ import static frc.team2412.robot.subsystems.ArmSubsystem.ArmConstants.PositionType.*; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import com.revrobotics.SparkMaxAbsoluteEncoder; @@ -25,7 +24,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team2412.robot.Robot; import frc.team2412.robot.sim.PhysicsSim; -import frc.team2412.robot.sim.SparkMaxSimProfile; public class ArmSubsystem extends SubsystemBase { From 260f058312e67f4ebb5bfe38530e5b192cd8054c Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Thu, 23 Mar 2023 16:34:25 -0700 Subject: [PATCH 3/5] make sim actually run --- src/main/java/frc/team2412/robot/Robot.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index a8fd0bec..29608ff3 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.team2412.robot.sim.PhysicsSim; +import frc.team2412.robot.subsystems.ArmSubsystem; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.auto.AutonomousChooser; import io.github.oblarg.oblog.Logger; @@ -170,6 +171,9 @@ public void simulationInit() { if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { subsystems.drivebaseSubsystem.simInit(sim); } + if (Subsystems.SubsystemConstants.ARM_ENABLED) { + subsystems.armSubsystem.simInit(sim); + } } @Override From a7a5626f7dcfa000961698139e408b0a3a9ae2a7 Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Thu, 23 Mar 2023 16:38:35 -0700 Subject: [PATCH 4/5] =?UTF-8?q?spotless=20=F0=9F=99=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/team2412/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index f76e6a6d..d049c4f7 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.team2412.robot.commands.arm.ManualArmOverrideOffCommand; import frc.team2412.robot.sim.PhysicsSim; -import frc.team2412.robot.subsystems.ArmSubsystem; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.auto.AutonomousChooser; import io.github.oblarg.oblog.Logger; From ea9a3afda72051883a01ca54c522434b3601ec0a Mon Sep 17 00:00:00 2001 From: DriverStation3 Date: Thu, 23 Mar 2023 17:01:01 -0700 Subject: [PATCH 5/5] make work on bot --- .../frc/team2412/robot/subsystems/ArmSubsystem.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java index fd32c1e0..1ae19ebe 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java @@ -77,9 +77,9 @@ public static class ArmConstants { public static final float ARM_FORWARD_LIMIT = 93.8f; // motor rotations public static final float ARM_REVERSE_LIMIT = 2; - public static final float WRIST_FORWARD_LIMIT = 72; + public static final float WRIST_FORWARD_LIMIT = 62; public static final float WRIST_REVERSE_LIMIT = 2; - public static final float WRIST_ENCODER_OFFSET = 0.227f; + public static final float WRIST_ENCODER_OFFSET = -0.22f; public static final double ARM_VELOCITY_TOLERANCE = 0.2; public static final double WRIST_VELOCITY_TOLERANCE = 0.2; @@ -131,9 +131,9 @@ public static enum PositionType { double prescoringWristAngle, double scoringWristAngle) { this.armAngle = armAngle; - this.retractedWristAngle = retractedWristAngle + WRIST_ENCODER_OFFSET; - this.prescoringWristAngle = prescoringWristAngle + WRIST_ENCODER_OFFSET; - this.scoringWristAngle = scoringWristAngle + WRIST_ENCODER_OFFSET; + this.retractedWristAngle = retractedWristAngle; + this.prescoringWristAngle = prescoringWristAngle; + this.scoringWristAngle = scoringWristAngle; } } } @@ -418,7 +418,7 @@ public double getElbowPosition() { * @return Current wrist encoder position */ public double getWristPosition() { - return wristEncoder.getPosition(); + return Math.max(wristEncoder.getPosition() + WRIST_ENCODER_OFFSET, 0); } /**