diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java index 808fac0f..07caca79 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java @@ -23,7 +23,22 @@ public ArmTeleop(DoubleSupplier joystickSupplier, Arm armSubsystem) { this.armSubsystem = armSubsystem; addRequirements(armSubsystem); } - + public void controllerMoveArm(DoubleSupplier rightJoystick) { + /* + // move the arm around based off the right joystick movement on the manipulator joystick + //use the trapezoid thingy from robot code 2023 + //Math below | Summary: Take in controller Y axis as a double then calculate amount of volts needed to pass to the arm and when to stop based off of the controller movement. Done so by finding the constraints of the arm, translating the controller numbers, and finding how many volts and when to stop using feedvolts and PID + double kgv = getKg(); + double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0); + double armPIDVolts = armPID.calculate(getArmPos(), state.position); + if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || + (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) { + forbFlag = true; + armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0); + */ + + + } // Called when the command is initially scheduled. @Override public void initialize() {} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 16a6c6c1..e040bb2a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -49,22 +49,7 @@ public void setArmGoal(double targetPosition, double targetVelocity) { targetPosition = getArmClampedGoal(targetPosition); } - public void controllerMoveArm(double rightJoystick) { - /* - // move the arm around based off the right joystick movement on the manipulator joystick - //use the trapezoid thingy from robot code 2023 - //Math below | Summary: Take in controller Y axis as a double then calculate amount of volts needed to pass to the arm and when to stop based off of the controller movement. Done so by finding the constraints of the arm, translating the controller numbers, and finding how many volts and when to stop using feedvolts and PID - double kgv = getKg(); - double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0); - double armPIDVolts = armPID.calculate(getArmPos(), state.position); - if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || - (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) { - forbFlag = true; - armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0); - */ - - - } + public double getArmClampedGoal(double goal) { //Find the limits of the arm. Used to move it and ensure that the arm does not move past the amount return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE, Constants.Arm.UPPER_ANGLE);