KeshavAnandCode/FTCDecodeMain
Archived
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

changed to radians, accounted for velo and acc

Browse Source
This commit is contained in:
Keshav Anand
2026-05-25 15:39:10 -07:00
parent e3105a339d
commit ed970eaf38
1 changed files with 32 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@@ -13,41 +14,49 @@ public class Turret {
private final double turretMin = 0.04; //TODO: Tune
private final double turretMax = 0.94; //TODO: Tune
private final double hVelK = 0.12; // TODO: Tune
private final double hAccK = 0.02; // TODO: Tune
public Turret (Robot rob){
private final double xVelK = 0.10; // TODO: Tune
private final double xAccK = 0.02; // TODO: Tune
private final double yVelK = 0.10; // TODO: Tune
private final double yAccK = 0.02; // TODO: Tune
public Turret(Robot rob) {
this.robot = rob;
}
public void trackGoal (double dx, double dy, double h) {
private double wrapAngle(double angle) {
while (angle > Math.PI) angle -= 2.0 * Math.PI;
while (angle < -Math.PI) angle += 2.0 * Math.PI;
return angle;
}
public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) {
// dx, dy, dz is target - robot
// h is the raw heading where 0 degrees is positive x in the system of x, y
while (h > 180) h -= 360;
while (h < -180) h += 360;
double fieldRelativeHeading = Math.toDegrees(Math.atan2(dy,dx)); // Angle assuming the robot is at zero degrees, CCW
double desiredAngle = fieldRelativeHeading - h; // Account for robot rotation
double angleDelta = desiredAngle - 180; // Subtract 180 as the neutral position is at 180 degrees
double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot
double predictedH = h + (hVel * hVelK) + (0.5 * hAcc * hAccK); // Positive bc h = robot heading
// Shift to -180 --> 180 scale
while (angleDelta > 180) angleDelta -= 360;
while (angleDelta < -180) angleDelta += 360;
predictedH = wrapAngle(predictedH);
double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx);
double desiredAngle = fieldRelativeHeading - predictedH;
double angleDelta = desiredAngle - Math.PI;
angleDelta = wrapAngle(angleDelta);
double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
double servoTicksFromNeutral = (angleDelta / 360.0) * (2 * servoTicksPer180);
double servoAngle = neutralPosition + servoTicksFromNeutral;
servoAngle = Range.clip(servoAngle, turretMin, turretMax);
robot.turr1.setPosition(servoAngle);
robot.turr2.setPosition(1.0 - servoAngle);
}
}