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

Fix CW turns

Browse Source
This commit is contained in:
Ryan Brott
2022-12-05 22:00:23 -08:00
parent 6f6a0c9771
commit b44d5e0ca5
2 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -297,12 +297,14 @@ public final class MecanumDrive {
private final TimeProfile profile; private final TimeProfile profile;
private double beginTs; private double beginTs;
private final boolean reversed;
private boolean active; private boolean active;
public TurnAction(Pose2d beginPose, double angle) { public TurnAction(Pose2d beginPose, double angle) {
this.beginPose = beginPose; this.beginPose = beginPose;
profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile); profile = new TimeProfile(Profiles.constantProfile(Math.abs(angle), 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
reversed = angle < 0;
} }
@Override @Override
@ -327,6 +329,10 @@ public final class MecanumDrive {
} }
DualNum<Time> x = profile.get(t); DualNum<Time> x = profile.get(t);
if (reversed) {
x = x.unaryMinus();
}
Pose2dDual<Time> txWorldTarget = Rotation2dDual.exp(x).times(beginPose); Pose2dDual<Time> txWorldTarget = Rotation2dDual.exp(x).times(beginPose);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel(); Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -317,12 +317,14 @@ public final class TankDrive {
private final TimeProfile profile; private final TimeProfile profile;
private double beginTs; private double beginTs;
private final boolean reversed;
private boolean active; private boolean active;
public TurnAction(Pose2d beginPose, double angle) { public TurnAction(Pose2d beginPose, double angle) {
this.beginPose = beginPose; this.beginPose = beginPose;
profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile); profile = new TimeProfile(Profiles.constantProfile(Math.abs(angle), 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
reversed = angle < 0;
} }
@Override @Override
@ -349,6 +351,9 @@ public final class TankDrive {
} }
DualNum<Time> x = profile.get(t); DualNum<Time> x = profile.get(t);
if (reversed) {
x = x.unaryMinus();
}
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel(); Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();