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

Fixed IK really

Browse Source
This commit is contained in:
aarav18
2022-01-22 15:58:34 -07:00
parent 6a44632d2a
commit 0ecae16cfe
2 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -86,7 +86,7 @@ public final class Constants {
public static final double LOWER_ARM_LENGTH = 50;
public static final double MAX_ARM_LENGTH = 100;
public static final double MIN_ARM_LENGTH = 5;
public static final double MIN_ARM_LENGTH = 0;
public static final double MOVE_SPEED = .2; // cm per second
src/main/java/frc4388/robot/subsystems/Climber.java
+16 -3
View File
@@ -101,19 +101,29 @@ public class Climber extends SubsystemBase {
double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
(2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
//elbowAngle = Math.PI - elbowAngle;
System.out.println("sa1: " + shoulderAngle);
System.out.println("ea1: " + elbowAngle);
// If the climber is resting on the robot base, rotate the upper arm in the direction of the target
if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH;
System.out.println("xDiff: " + xDiff);
elbowAngle = Math.atan(-yTarget / xDiff);
System.out.println("ea2: " + elbowAngle);
if(elbowAngle < 0) {
elbowAngle = Math.PI - Math.abs(elbowAngle);
}
if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
System.out.println("ea3: " + elbowAngle);
// Deal with negative wraparound
if(xDiff >= 0.d)
elbowAngle += Math.PI;
// if(xDiff >= 0.d)
// elbowAngle += Math.PI;
// System.out.println("ea4: " + elbowAngle);
}
angles[0] = shoulderAngle;
@@ -182,7 +192,10 @@ public class Climber extends SubsystemBase {
double[] testAngles5 = getJointAngles(-75, 0, 0);
System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]);
double[] testAngles6 = getJointAngles(60, 25, 0);
System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]);
double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle);
//setJointAngles(jointAngles);
setJointAngles(jointAngles);
}
}