diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index ec22a4d8..0b7905df 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -98,9 +98,36 @@ public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, Trapez } - public void maxacceleration(){ - armFeed.maxAchievableAcceleration(MAX_VOLTAGE, armEncoder.getVelocity()); + + //public void maxacceleration(){ + //armFeed.maxAchievableAcceleration(MAX_VOLTAGE, armEncoder.getVelocity()); + //} + + double currentVelocity = armEncoder.getVelocity(); + + public double maxacceleration(){ + double maxAccel = armFeed.maxAchievableAcceleration(MAX_VOLTAGE, currentVelocity); + return maxAccel; + } + + //public void maxvelocity(){ + //armFeed.maxAchievableVelocity(MAX_VOLTAGE, ); + //} + + public double calculateTrapTime(double goalAngle, double currentAngle){ + double distToCover = goalAngle - currentAngle; + double maxAccel = maxacceleration(); + + double timeOfAccelTriangle = MAX_FF_VEL / maxAccel; + double distCovrdTriangle = maxAccel / 2 * timeOfAccelTriangle; + + double distLeft = distToCover - (2 * distCovrdTriangle); + double timeOfRect = distLeft / MAX_FF_VEL; + + double answer = timeOfAccelTriangle*2 + timeOfRect; + return answer; } + // public void driveArm(double timeToTarget, TrapezoidProfile.State goalState) { // TrapezoidProfile.State setPoint = profile.calculate(timeToTarget, getCurrentArmState(), goalState);