SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

teleop enhancements fix

Browse Source
This commit is contained in:
brotherhobo
2024-07-29 22:15:07 -04:00
parent bee9f02577
commit b7ddb3465a
2 changed files with 60 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java
View File

@ -6,8 +6,6 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
/**
* This is the TeleOpEnhancements OpMode. It is an example usage of the TeleOp enhancements that
@ -27,15 +25,12 @@ public class TeleOpEnhancements extends OpMode {
private DcMotorEx rightFront;
private DcMotorEx rightRear;
private Vector driveVector;
private Vector headingVector;
/**
* This initializes the drive motors as well as the Follower and motion Vectors.
*/
@Override
public void init() {
follower = new Follower(hardwareMap, false);
follower = new Follower(hardwareMap);
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
@ -47,8 +42,7 @@ public class TeleOpEnhancements extends OpMode {
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveVector = new Vector();
headingVector = new Vector();
follower.startTeleopDrive();
}
/**
@ -57,15 +51,7 @@ public class TeleOpEnhancements extends OpMode {
*/
@Override
public void loop() {
driveVector.setOrthogonalComponents(-gamepad1.left_stick_y, -gamepad1.left_stick_x);
driveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1));
// TODO: if you want to make this field centric, then just remove this line
driveVector.rotateVector(follower.getPose().getHeading());
headingVector.setComponents(-gamepad1.right_stick_x, follower.getPose().getHeading());
follower.setMovementVectors(follower.getCentripetalForceCorrection(), headingVector, driveVector);
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
}

100
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View File

@ -82,9 +82,10 @@ public class Follower {
private boolean followingPathChain;
private boolean holdingPosition;
private boolean isBusy;
private boolean auto = true;
private boolean reachedParametricPathEnd;
private boolean holdPositionAtEnd;
private boolean teleopDrive;
private boolean fieldCentric;
private double maxPower = 1;
private double previousSmallTranslationalIntegral;
@ -97,6 +98,7 @@ public class Follower {
private long reachedParametricPathEndTime;
private double[] drivePowers;
private double[] teleopDriveValues;
private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()};
@ -144,19 +146,6 @@ public class Follower {
initialize();
}
/**
* This creates a new Follower given a HardwareMap and sets whether the Follower is being used
* in autonomous or teleop.
*
* @param hardwareMap HardwareMap required
* @param setAuto sets whether or not the Follower is being used in autonomous or teleop
*/
public Follower(HardwareMap hardwareMap, boolean setAuto) {
this.hardwareMap = hardwareMap;
setAuto(setAuto);
initialize();
}
/**
* This initializes the follower.
* In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are
@ -188,15 +177,12 @@ public class Follower {
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
}
for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) {
velocities.add(new Vector());
}
for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) {
accelerations.add(new Vector());
}
calculateAveragedVelocityAndAcceleration();
// TODO: Set this to true if you want to use field centric teleop drive.
fieldCentric = false;
dashboardPoseTracker = new DashboardPoseTracker(poseUpdater);
breakFollowing();
}
/**
@ -428,6 +414,14 @@ public class Follower {
followPath(pathChain, false);
}
/**
* This starts teleop drive control.
*/
public void startTeleopDrive() {
breakFollowing();
teleopDrive = true;
}
/**
* This calls an update to the PoseUpdater, which updates the robot's current position estimate.
* This also updates all the Follower's PIDFs, which updates the motor powers.
@ -439,7 +433,7 @@ public class Follower {
dashboardPoseTracker.update();
}
if (auto) {
if (!teleopDrive) {
if (holdingPosition) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
@ -499,7 +493,19 @@ public class Follower {
calculateAveragedVelocityAndAcceleration();
drivePowers = driveVectorScaler.getDrivePowers(teleOpMovementVectors[0], teleOpMovementVectors[1], teleOpMovementVectors[2], poseUpdater.getPose().getHeading());
Vector teleopDriveVector = new Vector();
Vector teleopHeadingVector = new Vector();
teleopDriveVector.setOrthogonalComponents(teleopDriveValues[0], teleopDriveValues[1]);
teleopDriveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1));
if (!fieldCentric) {
teleopDriveVector.rotateVector(getPose().getHeading());
}
teleopHeadingVector.setComponents(teleopDriveValues[2], getPose().getHeading());
drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading());
limitDrivePowers();
@ -509,6 +515,21 @@ public class Follower {
}
}
/**
* This sets the teleop drive vectors.
*
* @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric
* movement, this is the x-axis.
* @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric
* movement, this is the y-axis.
* @param heading determines the heading vector for the robot in teleop.
*/
public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) {
teleopDriveValues[0] = MathFunctions.clamp(forwardDrive, -1, 1);
teleopDriveValues[1] = MathFunctions.clamp(lateralDrive, -1, 1);
teleopDriveValues[2] = MathFunctions.clamp(heading, -1, 1);
}
/**
* This calculates an averaged approximate velocity and acceleration. This is used for a
* real-time correction of centripetal force, which is used in teleop.
@ -564,6 +585,7 @@ public class Follower {
* This resets the PIDFs and stops following the current Path.
*/
public void breakFollowing() {
teleopDrive = false;
holdingPosition = false;
isBusy = false;
reachedParametricPathEnd = false;
@ -594,6 +616,15 @@ public class Follower {
}
driveKalmanFilter.reset();
for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) {
velocities.add(new Vector());
}
for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) {
accelerations.add(new Vector());
}
calculateAveragedVelocityAndAcceleration();
teleopDriveValues = new double[3];
for (int i = 0; i < motors.size(); i++) {
motors.get(i).setPower(0);
}
@ -608,14 +639,6 @@ public class Follower {
return isBusy;
}
/**
* Sets the correctional, heading, and drive movement vectors for teleop enhancements.
* The correctional Vector only accounts for an approximated centripetal correction.
*/
public void setMovementVectors(Vector correctional, Vector heading, Vector drive) {
teleOpMovementVectors = new Vector[]{correctional, heading, drive};
}
/**
* This returns a Vector in the direction the robot must go to move along the path. This Vector
* takes into account the projected position of the robot to calculate how much power is needed.
@ -681,14 +704,14 @@ public class Follower {
previousRawDriveError = rawDriveError;
rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
double projection = 2 * driveErrors[2] - driveErrors[1];
double projection = 2 * driveErrors[1] - driveErrors[0];
driveKalmanFilter.update(rawDriveError - previousRawDriveError, projection);
for (int i = 0; i < driveErrors.length - 1; i++) {
driveErrors[i] = driveErrors[i + 1];
}
driveErrors[2] = driveKalmanFilter.getState();
driveErrors[1] = driveKalmanFilter.getState();
return driveKalmanFilter.getState();
}
@ -809,7 +832,7 @@ public class Follower {
public Vector getCentripetalForceCorrection() {
if (!useCentripetal) return new Vector();
double curvature;
if (auto) {
if (!teleopDrive) {
curvature = currentPath.getClosestPointCurvature();
} else {
double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
@ -832,15 +855,6 @@ public class Follower {
return closestPose;
}
/**
* This sets whether or not the Follower is being used in auto or teleop.
*
* @param set sets auto or not
*/
public void setAuto(boolean set) {
auto = set;
}
/**
* This returns whether the follower is at the parametric end of its current Path.
* The parametric end is determined by if the closest Point t-value is greater than some specified