From 080ede36c62d89089b2e7da64277621367fd1176 Mon Sep 17 00:00:00 2001 From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com> Date: Sun, 3 Mar 2024 13:04:28 -0800 Subject: [PATCH] made it look nice :) --- .../org/carlmontrobotics/subsystems/Arm.java | 25 +------------------ 1 file changed, 1 insertion(+), 24 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 94d447cb..3512f210 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -101,14 +101,10 @@ public Arm() { armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT); //two PIDs? - - SmartDashboard.putData("Arm", this); setPoint = getCurrentArmState(); - goalState = getCurrentArmState(); - setArmTarget(goalState.position); } @@ -135,14 +131,10 @@ public void periodic() { // SmartDashboard.putNumber("ArmPos", getArmPos()); - - autoCancelArmCommand(); driveArm(); } - - public void autoCancelArmCommand() { if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return; @@ -176,8 +168,6 @@ public void setArmTarget(double targetPos) { goalState.velocity = 0; } - - public void resetGoal() { double armPos = getArmPos(); setArmTarget(armPos); @@ -222,37 +212,24 @@ public double getArmPos() { ARM_DISCONT_RAD + 2 * Math.PI); } - public double getArmVel() { return armMasterEncoder.getVelocity(); } - public TrapezoidProfile.State getCurrentArmState() { return new TrapezoidProfile.State(getArmPos(), getArmVel()); } - public TrapezoidProfile.State getCurrentArmGoal() { return goalState; } - public boolean armAtSetpoint() { return Math.abs(getArmPos() - goalState.position) < POS_TOLERANCE_RAD && Math.abs(getArmVel() - goalState.velocity) < VEL_TOLERANCE_RAD_P_SEC; } - - public double getArmClampedGoal(double goal) { return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI), LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT); - } - - - - - - - + } } \ No newline at end of file