Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

finished dynamic limits (NOTE: fix actual soft limits)

Browse Source
This commit is contained in:
66945
2023-03-14 10:58:27 -06:00
parent 60a4df1e30
commit 1d32b65b2f
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Arm.java
+5 -2
View File
@@ -120,8 +120,11 @@ public class Arm extends SubsystemBase {
}
public void runPivotTele(double pivot, double tele) {
// TODO: tele has to go through some kind of transformation
if (pivot > 0 || tele < 0 || checkLimits(tele, getArmRotation())) {
double abs_pivot = Math.toRadians(getArmRotation() - 135);
double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
setRotVel(pivot);
setTeleVel(tele);
}