Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Corrected IK mistakes

Browse Source
This commit is contained in:
66945
2022-01-22 11:44:19 -07:00
parent 050d60ef36
commit 7386912b13
1 changed files with 4 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Climber.java
+4
View File
@@ -60,13 +60,17 @@ public class Climber extends SubsystemBase {
if(mag > ClimberConstants.MAX_ARM_LENGTH) {
xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH;
yTarget = (yTarget / mag) * ClimberConstants.MAX_ARM_LENGTH;
mag = ClimberConstants.MAX_ARM_LENGTH;
} else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
xTarget = (xTarget / mag) * ClimberConstants.MIN_ARM_LENGTH;
yTarget = (yTarget / mag) * ClimberConstants.MIN_ARM_LENGTH;
mag = ClimberConstants.MIN_ARM_LENGTH;
}
// The angle to the target point
double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable
if(xTarget < 0.d)
theta += Math.PI;
// Correct target position for tilt
xTarget = Math.cos(theta) * mag;