Skip to content

Commit

Permalink
Rotate robot diagrams in localizer files to reduce confusion
Browse files Browse the repository at this point in the history
  • Loading branch information
@Marlstar
Marlstar committed Nov 17, 2024
1 parent c5f33af commit 1056fe0
Show file tree
Hide file tree
Showing 16 changed files with 271 additions and 150 deletions.
46 changes: 36 additions & 10 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
@@ -1,19 +1,19 @@
package org.firstinspires.ftc.teamcode.pedroPathing.follower;

import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID;
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID;
Expand Down Expand Up @@ -365,6 +365,25 @@ public void holdPoint(BezierPoint point, double heading) {
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
}

/**
* This holds a Point.
*
* @param point the Point to stay at.
* @param heading the heading to face.
*/
public void holdPoint(Point point, double heading) {
holdPoint(new BezierPoint(point), heading);
}

/**
* This holds a Point.
*
* @param pose the Point (as a Pose) to stay at.
*/
public void holdPoint(Pose pose) {
holdPoint(new Point(pose), pose.getHeading());
}

/**
* This follows a Path.
* This also makes the Follower hold the last Point on the Path.
Expand Down Expand Up @@ -426,15 +445,22 @@ public void startTeleopDrive() {
}

/**
* 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.
* Calls an update to the PoseUpdater, which updates the robot's current position estimate.
*/
public void update() {
public void updatePose() {
poseUpdater.update();

if (drawOnDashboard) {
dashboardPoseTracker.update();
}
}

/**
* 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.
*/
public void update() {
updatePose();

if (!teleopDrive) {
if (currentPath != null) {
Expand Down Expand Up @@ -515,7 +541,7 @@ public void update() {
* 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.
* @param heading determines the heading vector for the robot in teleop.
*/
public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) {
setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true);
Expand All @@ -528,7 +554,7 @@ public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, d
* 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.
* @param heading determines the heading vector for the robot in teleop.
* @param robotCentric sets if the movement will be field or robot centric
*/
public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) {
Expand Down Expand Up @@ -719,7 +745,7 @@ public double getDriveVelocityError() {
Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError);

previousRawDriveError = rawDriveError;
rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));

double projection = 2 * driveErrors[1] - driveErrors[0];

Expand Down
10 changes: 9 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;

import androidx.annotation.NonNull;

import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;

Expand Down Expand Up @@ -44,7 +46,7 @@ public Pose(double setX, double setY) {
* This creates a new Pose with no inputs and 0 for all values.
*/
public Pose() {
this(0,0,0);
this(0, 0, 0);
}

/**
Expand Down Expand Up @@ -208,4 +210,10 @@ public boolean roughlyEquals(Pose pose) {
public Pose copy() {
return new Pose(getX(), getY(), getHeading());
}

@NonNull
@Override
public String toString() {
return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")";
}
}
31 changes: 17 additions & 14 deletions ...va/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,18 +21,18 @@
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
* forward (x positive)
* △
* |
* |
* /--------------\
* | |
* | |
* | || || |
* left (y positive) <--- | || || |
* | ____ |
* | ---- |
* \--------------/
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 7/20/2024
Expand Down Expand Up @@ -108,7 +108,10 @@ public OTOSLocalizer(HardwareMap map, Pose setStartPose) {
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h));
SparkFunOTOS.Pose2D rawPose = otos.getPosition();
Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);

return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
}

/**
Expand Down Expand Up @@ -219,4 +222,4 @@ public double getTurningMultiplier() {
*/
public void resetIMU() {
}
}
}
30 changes: 16 additions & 14 deletions ...rg/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,18 +23,18 @@
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || |
* | || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
* forward (x positive)
* △
* |
* |
* /--------------\
* | |
* | |
* | || |
* left (y positive) <--- | || |
* | ____ |
* | ---- |
* \--------------/
* With the pinpoint your readings will be used in mm
* to use inches ensure to divide your mm value by 25.4
* @author Logan Nash
Expand Down Expand Up @@ -96,8 +96,10 @@ public PinpointLocalizer(HardwareMap map, Pose setStartPose){
*/
@Override
public Pose getPose() {
Pose2D pose = odo.getPosition();
return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS));
Pose2D rawPose = odo.getPosition();
Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));

return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
}

/**
Expand Down
26 changes: 13 additions & 13 deletions ...rstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,18 +26,18 @@
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
* forward (x positive)
* △
* |
* |
* /--------------\
* | |
* | |
* | || || |
* left (y positive) <--- | || || |
* | ____ |
* | ---- |
* \--------------/
*
* @author Logan Nash
* @author Anyi Lin - 10158 Scott's Bots
Expand Down Expand Up @@ -313,4 +313,4 @@ public double getTurningMultiplier() {
public void resetIMU() {
imu.resetYaw();
}
}
}
24 changes: 12 additions & 12 deletions .../firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,18 +23,18 @@
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
* forward (x positive)
* △
* |
* |
* /--------------\
* | |
* | |
* | || || |
* left (y positive) <--- | || || |
* | ____ |
* | ---- |
* \--------------/
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
Expand Down
24 changes: 12 additions & 12 deletions ...rg/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,18 +26,18 @@
*
* forward on robot is the x positive direction
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || | ----> left (y positive)
* | |
* | |
* \--------------/
* |
* |
* V
* forward (x positive)
* forward (x positive)
* △
* |
* |
* /--------------\
* | |
* | |
* | || |
* left (y positive) <--- | || |
* | ____ |
* | ---- |
* \--------------/
*
* @author Anyi Lin - 10158 Scott's Bots
* @version 1.0, 4/2/2024
Expand Down
Loading

0 comments on commit 1056fe0

Please sign in to comment.