From f4b812cec7d414a55178c0603535f3caf0a264f1 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Sat, 2 Mar 2024 21:27:10 -0800 Subject: [PATCH] formatted constants --- .../java/org/carlmontrobotics/Constants.java | 83 ++++++++++--------- .../org/carlmontrobotics/RobotContainer.java | 15 ++-- .../org/carlmontrobotics/subsystems/Arm.java | 25 +++--- 3 files changed, 65 insertions(+), 58 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index e61c1ba3..e22a61a1 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units.*; @@ -24,72 +25,65 @@ public static final class Drivetrain { } public static final class Arm { - //Motor port + //ports public static final int ARM_MOTOR_PORT_1 = 7; public final static int ARM_MOTOR_PORT_2 = 8; - //all angles in rot here - //TODO: finish understand why this is broken public static final Measure INTAKE_ANGLE = Degrees.to(-1); - // USE RADIANS FOR THE ARM - public static final double intakeAngle = Math.toRadians(0); - public static final double ampAngle = Math.toRadians(103); - public static final double placeholderSpeakerAngle1 = Math.toRadians(24); - public static final double placeholderSpeakerAngle2 = Math.toRadians(24); - public static final double placeholderSpeakerAngle3 = Math.toRadians(24); - public static final double climberUpAngle = Math.toRadians(24); - public static final double climberDownAngle = Math.toRadians(24); + //config + public static final boolean MOTOR_INVERTED = true; //Todo: find all these (they are definetely wrong) + public static final boolean ENCODER_INVERTED = false; + //goal pos + // USE RADIANS FOR THE ARM + public static final double INTAKE_ANGLE = Units.degreesToRadians(0); + public static final double AMP_ANGLE = Units.degreesToRadians(103); + public static final double SPEAKER_ANGLE_1 = Units.degreesToRadians(24); //placeholder + public static final double SPEAKER_ANGLE_2 = Units.degreesToRadians(24); //placeholder + public static final double SPEAKER_ANGLE_3 = Units.degreesToRadians(24); //placeholder + public static final double CLIMBER_UP_ANGLE = Units.degreesToRadians(24); + public static final double CLIMBER_DOWN_ANGLE = Units.degreesToRadians(24); - //if needed - public static final double UPPER_ANGLE_LIMIT = Math.toRadians(70); - public static final double LOWER_ANGLE_LIMIT = Math.toRadians(0); - public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI; // Feedforward public static final double kS = 0.1; public static final double kG = 0.1; public static final double kV = 0.1; public static final double kA = 0.1; public static final double IZONE = 4; + // PID Constants - // placeholder numbers for now public static final double kP = 0.1; public static final double kI = 0.1; public static final double kD = 0.1; + //I assume to max vel and accel are in meters per second - public static final int MAX_VOLTAGE = 12; - - - //Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING - public static final double COM_ARM_LENGTH_METERS = 0.381 ; - public static final double ARM_MASS_KG = 9.59302503; + //trapezoidal public static final double MAX_FF_VEL = 1; // rot / s public static final double MAX_FF_ACCEL = 1; // rot / s^2 - //I assume to max vel and accel are in meters per second - public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL); + public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL); + public static final int MAX_VOLTAGE = 12; + public static final double ROTATION_TO_RAD = 2 * Math.PI; - //Arm buttons - public static final int raiseToSpeakerPodButton = Button.kY.value; - public static final int raiseToAmpButton = Button.kB.value; - public static final int raiseToSpeakerSafeButton = Button.kA.value; - public static final int raiseToSpeakerNextButton = Button.kX.value; - public static final int raiseToGroundButton = Button.kStart.value; - public static final int raiseToClimberButton = Button.kLeftBumper.value; - public static final int lowerToClimberButton = Button.kRightBumper.value; - //other - public static final boolean motorInverted = true; //Todo: find all these (they are definetely wrong) - public static final double rotationToRad = 2 * Math.PI; - public static final boolean encoderInverted = false; - public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; - public static final double posToleranceRad = 0; - public static final double velToleranceRadPSec = 0; + //boundaries + public static final double UPPER_ANGLE_LIMIT = Units.degreesToRadians(70); + public static final double LOWER_ANGLE_LIMIT = Units.degreesToRadians(0); + public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI; public static final double MARGIN_OF_ERROR = Math.PI/18; public static final double ARM_LOWER_LIMIT_RAD = -3.569 + MARGIN_OF_ERROR; public static final double ARM_UPPER_LIMIT_RAD = .36 - MARGIN_OF_ERROR; - + public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; public static final double ARM_DISCONTINUITY_RAD = (ARM_LOWER_LIMIT_RAD + ARM_UPPER_LIMIT_RAD) / 2 - Math.PI; + public static final double POS_TOLERANCE_RAD = 0; + public static final double VEL_TOLERANCE_RAD_PER_SEC = 0; + + //TODO: finish understand why this is broken public static final Measure INTAKE_ANGLE = Degrees.to(-1); + + //Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING + public static final double COM_ARM_LENGTH_METERS = 0.381 ; + public static final double ARM_MASS_KG = 9.59302503; } + public static final class IntakeShooter { //in set() speed public static final double idleSpeed = 0; @@ -105,6 +99,15 @@ public static final class Driver { } public static final class Manipulator { public static final int port = 1; + + //Arm buttons + public static final int RAISE_TO_SPEAKER_POD_BUTTON = Button.kY.value; + public static final int RAISE_TO_AMP_BUTTON = Button.kB.value; + public static final int RAISE_TO_SPEAKER_SAFE_BUTTON = Button.kA.value; + public static final int RAISE_TO_SPEAKER_NEXT_BUTTON = Button.kX.value; + public static final int RAISE_TO_GROUND_BUTTON = Button.kStart.value; + public static final int RAISE_TO_CLIMBER_BUTTON = Button.kLeftBumper.value; + public static final int LOWER_TO_CLIMER_BUTTON = Button.kRightBumper.value; } } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b743ae21..56fb691f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -7,7 +7,6 @@ // import org.carlmontrobotics.subsystems.*; import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; -import static org.carlmontrobotics.Constants.Arm.ampAngle; import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.subsystems.Arm; @@ -72,15 +71,15 @@ private void setBindingsManipulator() { //right joystick used for manual arm control // Speaker Buttons - new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerPodButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle1);})); - new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerNextButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle2);})); - new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerSafeButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle3);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_POD_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SPEAKER_ANGLE_1);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_NEXT_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SPEAKER_ANGLE_2);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_SAFE_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SPEAKER_ANGLE_3);})); // Amp and Intake Buttons - new JoystickButton(manipulatorController, Constants.Arm.raiseToAmpButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.ampAngle);})); - new JoystickButton(manipulatorController, Constants.Arm.raiseToGroundButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.intakeAngle);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_AMP_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.AMP_ANGLE);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_GROUND_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.INTAKE_ANGLE);})); // Cimber Buttons - new JoystickButton(manipulatorController, Constants.Arm.raiseToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberUpAngle);})); - new JoystickButton(manipulatorController, Constants.Arm.lowerToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberDownAngle);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_UP_ANGLE);})); + new JoystickButton(manipulatorController, Constants.OI.Manipulator.LOWER_TO_CLIMER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_DOWN_ANGLE);})); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 3b71a425..67931779 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -51,7 +51,7 @@ public class Arm extends SubsystemBase { private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(kS, kV, kA); private final SparkPIDController armPID1 = armMotor1.getPIDController(); private final SparkPIDController armPID2 = armMotor2.getPIDController(); - private TrapezoidProfile armProfile = new TrapezoidProfile(trapConstraints); + private TrapezoidProfile armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); private Timer armProfileTimer = new Timer(); TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later @@ -60,13 +60,13 @@ public class Arm extends SubsystemBase { public Arm() { // weird math stuff - armMotor1.setInverted(motorInverted); + armMotor1.setInverted(MOTOR_INVERTED); armMotor1.setIdleMode(IdleMode.kBrake); - armMotor2.setInverted(motorInverted); + armMotor2.setInverted(MOTOR_INVERTED); armMotor2.setIdleMode(IdleMode.kBrake); - armEncoder.setPositionConversionFactor(rotationToRad); - armEncoder.setVelocityConversionFactor(rotationToRad); - armEncoder.setInverted(encoderInverted); + armEncoder.setPositionConversionFactor(ROTATION_TO_RAD); + armEncoder.setVelocityConversionFactor(ROTATION_TO_RAD); + armEncoder.setInverted(ENCODER_INVERTED); armMotor2.follow(armMotor1); @@ -82,8 +82,8 @@ public Arm() { // SmartDashboard.putNumber("Arm Max Vel", MAX_FF_VEL ); SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); - SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad); - SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec); + SmartDashboard.putNumber("Arm Tolerance Pos", POS_TOLERANCE_RAD); + SmartDashboard.putNumber("Arm Tolerance Vel", VEL_TOLERANCE_RAD_PER_SEC); } @Override @@ -119,6 +119,11 @@ public void periodic() { autoCancelArmCommand(); } + public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, TrapezoidProfile.State currentPoint, TrapezoidProfile.State goalState) { + return armProfile.calculate(goalSeconds, currentPoint, goalState); + + } + public void autoCancelArmCommand() { if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return; @@ -144,7 +149,7 @@ public void driveArm(double goalAngle){ public void setArmTarget(double targetPos) { targetPos = getArmClampedGoal(targetPos); - armProfile = new TrapezoidProfile(trapConstraints); + armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); armProfileTimer.reset(); goalState.position = targetPos; @@ -156,7 +161,7 @@ public void setArmTarget(double targetPos) { public void resetGoal() { double armPos = getArmPos(); - armProfile = new TrapezoidProfile(trapConstraints); + armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); }