Skip to content

Commit

Permalink
teleop enhancements fix
Browse files Browse the repository at this point in the history
  • Loading branch information
@AnyiLin
AnyiLin committed Jul 30, 2024
1 parent bee9f02 commit b7ddb34
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 60 deletions.
20 changes: 3 additions & 17 deletions ...rc/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@
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
Expand All @@ -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");
Expand All @@ -47,8 +42,7 @@ public void init() {
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

driveVector = new Vector();
headingVector = new Vector();
follower.startTeleopDrive();
}

/**
Expand All @@ -57,15 +51,7 @@ public void init() {
*/
@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 changes: 57 additions & 43 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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()};

Expand Down Expand Up @@ -144,19 +146,6 @@ public Follower(HardwareMap hardwareMap) {
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
Expand Down Expand Up @@ -188,15 +177,12 @@ public void initialize() {
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();
}

/**
Expand Down Expand Up @@ -428,6 +414,14 @@ public void followPath(PathChain pathChain) {
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.
Expand All @@ -439,7 +433,7 @@ public void update() {
dashboardPoseTracker.update();
}

if (auto) {
if (!teleopDrive) {
if (holdingPosition) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);

Expand Down Expand Up @@ -499,7 +493,19 @@ public void update() {

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();

Expand All @@ -509,6 +515,21 @@ public void update() {
}
}

/**
* 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.
Expand Down Expand Up @@ -564,6 +585,7 @@ public void updateCallbacks() {
* This resets the PIDFs and stops following the current Path.
*/
public void breakFollowing() {
teleopDrive = false;
holdingPosition = false;
isBusy = false;
reachedParametricPathEnd = false;
Expand Down Expand Up @@ -594,6 +616,15 @@ public void breakFollowing() {
}
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);
}
Expand All @@ -608,14 +639,6 @@ public boolean isBusy() {
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.
Expand Down Expand Up @@ -681,14 +704,14 @@ public double getDriveVelocityError() {
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();
}
Expand Down Expand Up @@ -809,7 +832,7 @@ public Vector getTranslationalError() {
public Vector getCentripetalForceCorrection() {
if (!useCentripetal) return new Vector();
double curvature;
if (auto) {
if (!teleopDrive) {
curvature = currentPath.getClosestPointCurvature();
} else {
double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
Expand All @@ -832,15 +855,6 @@ public Pose getClosestPose() {
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
Expand Down

0 comments on commit b7ddb34

Please sign in to comment.