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

improved dynamic soft limits

Browse Source
This commit is contained in:
66945
2023-03-14 10:54:00 -06:00
parent 6027b5ffb2
commit 4af2986ea6
2 changed files with 7 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/ArmCommand.java
-2
View File
@@ -4,8 +4,6 @@
package frc4388.robot.commands; package frc4388.robot.commands;
import org.opencv.osgi.OpenCVInterface;
import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Arm;
import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.Claw;
src/main/java/frc4388/robot/subsystems/Arm.java
+7 -13
View File
@@ -112,21 +112,18 @@ public class Arm extends SubsystemBase {
} }
public double getArmLength() { public double getArmLength() {
return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) / return m_tele.getSelectedSensorPosition();
(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT);
} }
public double getArmRotation() { public double getArmRotation() {
return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) / return m_pivotEncoder.getAbsolutePosition();
(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
} }
public void runPivotTele(double pivot, double tele) { public void runPivotTele(double pivot, double tele) {
double rot = 0; // TODO: tele has to go through some kind of transformation
if (pivot > 0 || tele < 0 || checkLimits(tele, getArmRotation())) {
if (checkLimits(tele, rot)) { setRotVel(pivot);
armSetRotation(pivot); setTeleVel(tele);
armSetLength(tele);
} }
} }
@@ -142,10 +139,7 @@ public class Arm extends SubsystemBase {
var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
if (y < minHeight) return y < minHeight;
return false;
return true;
} }
boolean tele_softLimit = false; boolean tele_softLimit = false;