From 3160103d6e453656063f2e0e6f5e3240ebe3962b Mon Sep 17 00:00:00 2001 From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com> Date: Fri, 1 Mar 2024 23:33:31 -0800 Subject: [PATCH] fixed driveArm --- .../carlmontrobotics/commands/ArmTeleop.java | 2 +- .../org/carlmontrobotics/subsystems/Arm.java | 17 +++++++---------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java index 31df4920..0e9af38e 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java @@ -53,7 +53,7 @@ public void execute() { goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT); goalState = new TrapezoidProfile.State(goalArmRad, 0); TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(armTimer.get(), armSubsystem.getCurrentArmState(), goalState); - armSubsystem.COMBINE_PID_FF_TRAPEZOID(setpoint); + armSubsystem.driveArm(setpoint); lastTime = currTime; } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 4f57e347..39656d56 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -135,17 +135,14 @@ public void autoCancelArmCommand() { } //#region Drive Methods - - public void driveArm(double goalAngle) { - double targetRPS = SmartDashboard.getNumber("Shooter RPS", 0); - TrapezoidProfile.State setPoint = armProfile.calculate(kDt, getCurrentArmState(), goalState); - double armFeedVolts = armFeed.calculate(goalState.velocity, 0); - double feed = armFeed.calculate(targetRPS); - armPID1.setReference(targetRPS * 60, CANSparkBase.ControlType.kVelocity, 0, feed); - armPID2.setReference(targetRPS * 60, CANSparkBase.ControlType.kVelocity, 0, feed); + public void driveArm(double goalAngle ){ + TrapezoidProfile.State goalState = new TrapezoidProfile.State(goalAngle, 0); + TrapezoidProfile.State setPoint = armProfile.calculate(kDt, getCurrentArmState(), goalState); + double armFeedVolts = armFeed.calculate(goalState.velocity, 0); + + armPID1.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts); } - public void setArmTarget(double targetPos) { targetPos = getArmClampedGoal(targetPos); @@ -161,7 +158,7 @@ public void setArmTarget(double targetPos) { public void resetGoal() { double armPos = getArmPos(); - armProfile = new TrapezoidProfile(trapConstraints, ); + armProfile = new TrapezoidProfile(trapConstraints); }