From a84323d528f976118d0e74e4dcbb7188fd8b74c4 Mon Sep 17 00:00:00 2001 From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com> Date: Sat, 2 Mar 2024 21:40:30 -0800 Subject: [PATCH 1/6] delted timer --- src/main/java/org/carlmontrobotics/commands/ArmTeleop.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java index 07f27e53..c06fb9e8 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java @@ -25,7 +25,7 @@ public class ArmTeleop extends Command { private final DoubleSupplier joystick; private final Arm armSubsystem; private double lastTime = 0; - private Timer armTimer = new Timer(); + TrapezoidProfile.State goalState; /** Creates a new ArmTeleop. */ public ArmTeleop(Arm armSubsystem, DoubleSupplier joystickSupplier) { From 67be12a94e4fed50120a0060998de4b04a3eaa5b Mon Sep 17 00:00:00 2001 From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com> Date: Sat, 2 Mar 2024 21:41:41 -0800 Subject: [PATCH 2/6] public -->private --- src/main/java/org/carlmontrobotics/subsystems/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 6e4af17a..f4ea019d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -143,7 +143,7 @@ public void autoCancelArmCommand() { } //#region Drive Methods - public void driveArm(double goalAngle){ + private 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); From 814cd6463ed2f3244afd93658841d4ee50723edf Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Sat, 2 Mar 2024 21:42:12 -0800 Subject: [PATCH 3/6] deleted smartdashboards --- src/main/java/org/carlmontrobotics/subsystems/Arm.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 6e4af17a..add8e6a0 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -84,11 +84,6 @@ public Arm() { //armProfileTimer.start(); <-- don't neeed timer anymore setArmTarget(goalState.position); - - // 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", POS_TOLERANCE_RAD); - SmartDashboard.putNumber("Arm Tolerance Vel", VEL_TOLERANCE_RAD_P_SEC); } @Override From fc75e06f2363e5cb2c326159f23676a59e77f7aa Mon Sep 17 00:00:00 2001 From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com> Date: Sat, 2 Mar 2024 21:42:20 -0800 Subject: [PATCH 4/6] public now it was bad --- src/main/java/org/carlmontrobotics/subsystems/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index f4ea019d..6e4af17a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -143,7 +143,7 @@ public void autoCancelArmCommand() { } //#region Drive Methods - private void driveArm(double goalAngle){ + 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); From 7ce82f79e1aa74433adec4969d74bf0920229f9b Mon Sep 17 00:00:00 2001 From: Brandon Date: Sat, 2 Mar 2024 21:45:56 -0800 Subject: [PATCH 5/6] fixed arm teleop --- src/main/java/org/carlmontrobotics/commands/ArmTeleop.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java index c06fb9e8..5e769975 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java @@ -52,7 +52,7 @@ public void execute() { double goalArmRad = goalState.position + speeds * deltaT; 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); + TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(0.02, armSubsystem.getCurrentArmState(), goalState); armSubsystem.driveArm(setpoint.position); lastTime = currTime; } @@ -71,8 +71,6 @@ public double getRequestedSpeeds() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - armTimer.stop(); - armTimer.reset(); } // Returns true when the command should end. From 30e00bf410b01350b77ed245accea38b6ee1c116 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Sat, 2 Mar 2024 21:46:27 -0800 Subject: [PATCH 6/6] no timer --- src/main/java/org/carlmontrobotics/commands/ArmTeleop.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java index c06fb9e8..5e769975 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java @@ -52,7 +52,7 @@ public void execute() { double goalArmRad = goalState.position + speeds * deltaT; 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); + TrapezoidProfile.State setpoint = armSubsystem.calculateCustomSetPoint(0.02, armSubsystem.getCurrentArmState(), goalState); armSubsystem.driveArm(setpoint.position); lastTime = currTime; } @@ -71,8 +71,6 @@ public double getRequestedSpeeds() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - armTimer.stop(); - armTimer.reset(); } // Returns true when the command should end.