Skip to content

Commit 62e752e

Browse files
kevinfreikevinfrei
committed
added drive base mechanum fixes from 16750
1 parent 75a428a commit 62e752e

File tree

6 files changed

+177
-23
lines changed

6 files changed

+177
-23
lines changed

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,5 +47,6 @@ public static class GlobalSettings {
4747
public static double STRAIGHTEN_SCALE_FACTOR = 0.25;
4848
public static double STRAIGHTEN_RANGE = .15; // Fraction of 45 degrees...
4949
public static double TRIGGER_THRESHOLD = 0.7;
50+
public static double STRAIGHTEN_DEAD_ZONE = 0.08;
5051
}
5152
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
package org.firstinspires.ftc.ptechnodactyl.commands;
2+
3+
import com.technototes.library.command.Command;
4+
import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem;
5+
6+
public class DrivingCommands {
7+
8+
public static Command NormalDriving(DrivebaseSubsystem ds) {
9+
return Command.create(ds::setNormalMode);
10+
}
11+
12+
public static Command SnailDriving(DrivebaseSubsystem ds) {
13+
return Command.create(ds::setSnailMode);
14+
}
15+
16+
public static Command TurboDriving(DrivebaseSubsystem ds) {
17+
return Command.create(ds::setTurboMode);
18+
}
19+
20+
public static Command NormalSpeed(DrivebaseSubsystem ds) {
21+
return Command.create(ds::auto);
22+
}
23+
24+
public static Command SlowSpeed(DrivebaseSubsystem ds) {
25+
return Command.create(ds::slow);
26+
}
27+
28+
public static Command FastSpeed(DrivebaseSubsystem ds) {
29+
return Command.create(ds::fast);
30+
}
31+
32+
public static Command ResetGyro(DrivebaseSubsystem ds) {
33+
return Command.create(ds::setExternalHeading, 0.0);
34+
}
35+
36+
public static Command RecordHeading(DrivebaseSubsystem drive) {
37+
return Command.create(drive::saveHeading);
38+
}
39+
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java‎

Lines changed: 46 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -5,68 +5,98 @@
55
import com.acmerobotics.roadrunner.geometry.Vector2d;
66
import com.technototes.library.command.Command;
77
import com.technototes.library.control.Stick;
8+
import com.technototes.library.logger.Loggable;
89
import com.technototes.library.util.MathUtils;
910
import java.util.function.BooleanSupplier;
1011
import java.util.function.DoubleSupplier;
1112
import org.firstinspires.ftc.ptechnodactyl.Setup;
1213
import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem;
1314

14-
public class JoystickDriveCommand implements Command {
15+
public class JoystickDriveCommand implements Command, Loggable {
1516

1617
public DrivebaseSubsystem subsystem;
1718
public DoubleSupplier x, y, r;
1819
public BooleanSupplier watchTrigger;
1920
public double targetHeadingRads;
2021
public DoubleSupplier driveStraighten;
22+
public DoubleSupplier drive45;
23+
public boolean driverDriving;
24+
public boolean operatorDriving;
2125

2226
public JoystickDriveCommand(
2327
DrivebaseSubsystem sub,
2428
Stick xyStick,
2529
Stick rotStick,
26-
DoubleSupplier straightDrive
30+
DoubleSupplier strtDrive,
31+
DoubleSupplier angleDrive
2732
) {
2833
addRequirements(sub);
2934
subsystem = sub;
3035
x = xyStick.getXSupplier();
3136
y = xyStick.getYSupplier();
3237
r = rotStick.getXSupplier();
3338
targetHeadingRads = -sub.getExternalHeading();
34-
driveStraighten = straightDrive;
39+
driveStraighten = strtDrive;
40+
drive45 = angleDrive;
41+
driverDriving = true;
42+
operatorDriving = false;
3543
}
3644

3745
// Use this constructor if you don't want auto-straightening
3846
public JoystickDriveCommand(DrivebaseSubsystem sub, Stick xyStick, Stick rotStick) {
39-
this(sub, xyStick, rotStick, null);
47+
this(sub, xyStick, rotStick, null, null);
4048
}
4149

4250
// This will make the bot snap to an angle, if the 'straighten' button is pressed
4351
// Otherwise, it just reads the rotation value from the rotation stick
4452
private double getRotation(double headingInRads) {
4553
// Check to see if we're trying to straighten the robot
46-
if (
47-
driveStraighten == null ||
48-
driveStraighten.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD
49-
) {
54+
double normalized = 0.0;
55+
boolean straightTrigger;
56+
boolean fortyfiveTrigger;
57+
straightTrigger = isTriggered(driveStraighten);
58+
fortyfiveTrigger = isTriggered(drive45);
59+
if (!straightTrigger && !fortyfiveTrigger) {
5060
// No straighten override: return the stick value
5161
// (with some adjustment...)
5262
return -Math.pow(r.getAsDouble(), 3) * subsystem.speed;
53-
} else {
63+
}
64+
if (straightTrigger) {
5465
// headingInRads is [0-2pi]
5566
double heading = -Math.toDegrees(headingInRads);
67+
// Snap to the closest 90 or 270 degree angle (for going through the depot)
5668
double close = MathUtils.closestTo(heading, 0, 90, 180, 270, 360);
5769
double offBy = close - heading;
5870
// Normalize the error to -1 to 1
59-
double normalized = Math.max(Math.min(offBy / 45, 1.), -1.);
71+
normalized = Math.max(Math.min(offBy / 45, 1.), -1.);
6072
// Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem)
61-
if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_RANGE) {
73+
if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) {
6274
return 0.0;
6375
}
64-
// Fix the range to be from (abs) dead_zone => 1 to scal from 0 to 1
65-
// Scale it by the cube root, the scale that down by 30%
66-
// .9 (about 40 degrees off) provides .96 power => .288
67-
// .1 (about 5 degrees off) provides .46 power => .14
68-
return normalized * Setup.GlobalSettings.STRAIGHTEN_SCALE_FACTOR; // Math.cbrt(normalized) * Setup.GlobalSettings.STRAIGHTEN_SCALE_FACTOR;
76+
} else {
77+
// headingInRads is [0-2pi]
78+
double heading45 = -Math.toDegrees(headingInRads);
79+
// Snap to the closest 90 or 270 degree angle (for going through the depot)
80+
double close45 = MathUtils.closestTo(heading45, 45, 135, 225, 315);
81+
double offBy45 = close45 - heading45;
82+
// Normalize the error to -1 to 1
83+
normalized = Math.max(Math.min(offBy45 / 45, 1.), -1.);
84+
// Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem)
85+
if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) {
86+
return 0.0;
87+
}
88+
}
89+
// Scale it by the cube root, the scale that down by 30%
90+
// .9 (about 40 degrees off) provides .96 power => .288
91+
// .1 (about 5 degrees off) provides .46 power => .14
92+
return Math.cbrt(normalized) * 0.3;
93+
}
94+
95+
public static boolean isTriggered(DoubleSupplier ds) {
96+
if (ds == null || ds.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD) {
97+
return false;
6998
}
99+
return true;
70100
}
71101

72102
@Override

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java‎

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
import com.technototes.library.control.Stick;
88
import org.firstinspires.ftc.ptechnodactyl.Robot;
99
import org.firstinspires.ftc.ptechnodactyl.Setup;
10-
import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd;
10+
import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands;
1111
import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand;
1212

1313
public class DriverController {
@@ -19,10 +19,12 @@ public class DriverController {
1919
public CommandButton resetGyroButton, turboButton, snailButton;
2020
public CommandButton override;
2121
public CommandAxis driveStraighten;
22+
public CommandAxis drive45;
2223

2324
public DriverController(CommandGamepad g, Robot r) {
2425
this.robot = r;
2526
gamepad = g;
27+
override = g.leftTrigger.getAsButton(0.5);
2628

2729
AssignNamedControllerButton();
2830
if (Setup.Connected.DRIVEBASE) {
@@ -35,6 +37,7 @@ private void AssignNamedControllerButton() {
3537
driveLeftStick = gamepad.leftStick;
3638
driveRightStick = gamepad.rightStick;
3739
driveStraighten = gamepad.rightTrigger;
40+
drive45 = gamepad.leftTrigger;
3841
turboButton = gamepad.leftBumper;
3942
snailButton = gamepad.rightBumper;
4043
}
@@ -45,14 +48,15 @@ public void bindDriveControls() {
4548
robot.drivebaseSubsystem,
4649
driveLeftStick,
4750
driveRightStick,
48-
driveStraighten
51+
driveStraighten,
52+
drive45
4953
)
5054
);
51-
turboButton.whenPressed(EZCmd.Drive.TurboMode(robot.drivebaseSubsystem));
52-
turboButton.whenReleased(EZCmd.Drive.NormalMode(robot.drivebaseSubsystem));
53-
snailButton.whenPressed(EZCmd.Drive.SnailMode(robot.drivebaseSubsystem));
54-
snailButton.whenReleased(EZCmd.Drive.NormalMode(robot.drivebaseSubsystem));
5555

56-
resetGyroButton.whenPressed(EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem));
56+
turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem));
57+
turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem));
58+
snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem));
59+
snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem));
60+
resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem));
5761
}
5862
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
package org.firstinspires.ftc.ptechnodactyl.helpers;
2+
3+
import com.acmerobotics.dashboard.config.Config;
4+
import com.acmerobotics.roadrunner.geometry.Pose2d;
5+
import org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity;
6+
7+
@Config
8+
public class HeadingHelper {
9+
10+
public static double DEFAULT_START_HEADING = 180;
11+
public static double DEFAULT_START_X = 53;
12+
public static double DEFAULT_START_Y = 63;
13+
public static int EXPIRATION_TIME = 20;
14+
15+
public double headingUpdateTime;
16+
public double lastHeading;
17+
public double lastXPosition;
18+
public double lastYPosition;
19+
20+
public HeadingHelper(double lastX, double lastY, double heading) {
21+
headingUpdateTime = System.currentTimeMillis() / 1000.0;
22+
lastXPosition = lastX;
23+
lastYPosition = lastY;
24+
lastHeading = heading;
25+
}
26+
27+
public static void saveHeading(double x, double y, double h) {
28+
FtcRobotControllerActivity.SaveBetweenRuns = new HeadingHelper(x, y, h);
29+
}
30+
31+
public static void savePose(Pose2d p) {
32+
saveHeading(p.getX(), p.getY(), p.getHeading());
33+
}
34+
35+
public static void clearSavedInfo() {
36+
FtcRobotControllerActivity.SaveBetweenRuns = null;
37+
}
38+
39+
public static boolean validHeading() {
40+
HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
41+
if (hh == null) {
42+
return false;
43+
}
44+
double now = System.currentTimeMillis() / 1000.0;
45+
return now < hh.headingUpdateTime + EXPIRATION_TIME;
46+
}
47+
48+
public static Pose2d getSavedPose() {
49+
return new Pose2d(getSavedX(), getSavedY(), getSavedHeading());
50+
}
51+
52+
public static double getSavedHeading() {
53+
HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
54+
if (hh != null) {
55+
return hh.lastHeading;
56+
}
57+
return DEFAULT_START_HEADING;
58+
}
59+
60+
public static double getSavedX() {
61+
HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
62+
if (hh != null) {
63+
return hh.lastXPosition;
64+
}
65+
return DEFAULT_START_X;
66+
}
67+
68+
public static double getSavedY() {
69+
HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns;
70+
if (hh != null) {
71+
return hh.lastYPosition;
72+
}
73+
return DEFAULT_START_Y;
74+
}
75+
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
import com.technototes.path.subsystem.PathingMecanumDrivebaseSubsystem;
1414
import java.util.function.Supplier;
1515
import org.firstinspires.ftc.ptechnodactyl.Setup;
16+
import org.firstinspires.ftc.ptechnodactyl.helpers.HeadingHelper;
1617

1718
public class DrivebaseSubsystem
1819
extends PathingMecanumDrivebaseSubsystem
@@ -188,6 +189,10 @@ public void setSnailMode() {
188189
Turbo = false;
189190
}
190191

192+
public void saveHeading() {
193+
HeadingHelper.saveHeading(get().getX(), get().getY(), gyro.getHeading());
194+
}
195+
191196
public void setTurboMode() {
192197
Turbo = true;
193198
Snail = false;

0 commit comments

Comments
 (0)