Fine tuninng the constant and added more robust examples
This commit is contained in:
@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExampleThree", group = "Autonomous Pathing Tuning")
|
||||
@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning")
|
||||
public class AsherPathV1 extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
|
||||
@ -33,7 +33,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample", group = "Autonomous Pathing Tuning")
|
||||
@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExample extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
@ -41,7 +41,7 @@ public class AutoExample extends OpMode {
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(12.0, 11, 90);
|
||||
private final Pose startPose = new Pose(0.0, 20.0, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
@ -59,46 +59,11 @@ public class AutoExample extends OpMode {
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(12.804, 11.223, Point.CARTESIAN),
|
||||
new Point(12.804, 42.362, Point.CARTESIAN)
|
||||
new Point(0.000, 20.000, Point.CARTESIAN),
|
||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierCurve(
|
||||
new Point(12.804, 42.362, Point.CARTESIAN),
|
||||
new Point(11.381, 57.379, Point.CARTESIAN),
|
||||
new Point(31.614, 56.588, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(31.614, 56.588, Point.CARTESIAN),
|
||||
new Point(51.214, 56.746, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierCurve(
|
||||
new Point(51.214, 56.746, Point.CARTESIAN),
|
||||
new Point(64.334, 58.643, Point.CARTESIAN),
|
||||
new Point(61.172, 45.524, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.addPath(
|
||||
// Line 5
|
||||
new BezierCurve(
|
||||
new Point(61.172, 45.524, Point.CARTESIAN),
|
||||
new Point(36.198, 26.239, Point.CARTESIAN),
|
||||
new Point(19.759, 11.065, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
@ -0,0 +1,106 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
|
||||
/**
|
||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||
* heading is to be expected.
|
||||
*
|
||||
* @author Anyi Lin - 10158 Scott's Bots
|
||||
* @author Aaron Yang - 10158 Scott's Bots
|
||||
* @author Harrison Womack - 10158 Scott's Bots
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleFour extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
private Follower follower;
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(12,60, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
* initializes the FTC Dashboard telemetry.
|
||||
*/
|
||||
@Override
|
||||
public void init() {
|
||||
follower = new Follower(hardwareMap);
|
||||
|
||||
follower.setMaxPower(.45);
|
||||
|
||||
follower.setStartingPose(startPose);
|
||||
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(12.000, 60.000, Point.CARTESIAN),
|
||||
new Point(60.000, 60.000, Point.CARTESIAN),
|
||||
new Point(60.000, 12.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(60.000, 12.000, Point.CARTESIAN),
|
||||
new Point(40.000, 12.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierCurve(
|
||||
new Point(40.000, 12.000, Point.CARTESIAN),
|
||||
new Point(35.000, 35.000, Point.CARTESIAN),
|
||||
new Point(12.000, 35.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 4
|
||||
new BezierLine(
|
||||
new Point(12.000, 35.000, Point.CARTESIAN),
|
||||
new Point(12.000, 60.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
telemetryA.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||
* the Telemetry, as well as the FTC Dashboard.
|
||||
*/
|
||||
@Override
|
||||
public void loop() {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
follower.followPath(path);
|
||||
}
|
||||
follower.telemetryDebug(telemetryA);
|
||||
}
|
||||
}
|
||||
@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExampleThree", group = "Autonomous Pathing Tuning")
|
||||
@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleThree extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
@ -34,7 +34,7 @@ public class AutoExampleThree extends OpMode {
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(7.467869222096955, 59.74295377677565);
|
||||
private final Pose startPose = new Pose(10,45, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
@ -52,21 +52,21 @@ public class AutoExampleThree extends OpMode {
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierCurve(
|
||||
new Point(7.468, 59.743, Point.CARTESIAN),
|
||||
new Point(67.860, 12.014, Point.CARTESIAN),
|
||||
new Point(101.790, 36.041, Point.CARTESIAN),
|
||||
new Point(68.347, 22.891, Point.CARTESIAN)
|
||||
new Point(10.000, 45.000, Point.CARTESIAN),
|
||||
new Point(45.000, 45.000, Point.CARTESIAN),
|
||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation()
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(68.347, 22.891, Point.CARTESIAN),
|
||||
new Point(13.637, 24.352, Point.CARTESIAN)
|
||||
new Point(50.000, 20.000, Point.CARTESIAN),
|
||||
new Point(10.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setTangentHeadingInterpolation().build();
|
||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
|
||||
@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||
* @version 1.0, 3/12/2024
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(name = "AutoExampleTwo", group = "Autonomous Pathing Tuning")
|
||||
@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning")
|
||||
public class AutoExampleTwo extends OpMode {
|
||||
private Telemetry telemetryA;
|
||||
|
||||
@ -34,7 +34,7 @@ public class AutoExampleTwo extends OpMode {
|
||||
|
||||
private PathChain path;
|
||||
|
||||
private final Pose startPose = new Pose(10.0, 40, 90);
|
||||
private final Pose startPose = new Pose(10.0, 45, 0);
|
||||
|
||||
/**
|
||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||
@ -51,28 +51,14 @@ public class AutoExampleTwo extends OpMode {
|
||||
path = follower.pathBuilder()
|
||||
.addPath(
|
||||
// Line 1
|
||||
new BezierLine(
|
||||
new Point(10.000, 40.000, Point.CARTESIAN),
|
||||
new Point(60.000, 40.000, Point.CARTESIAN)
|
||||
new BezierCurve(
|
||||
new Point(10.000, 45.000, Point.CARTESIAN),
|
||||
new Point(45.000, 45.000, Point.CARTESIAN),
|
||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 2
|
||||
new BezierLine(
|
||||
new Point(60.000, 40.000, Point.CARTESIAN),
|
||||
new Point(60.000, 25.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
||||
.addPath(
|
||||
// Line 3
|
||||
new BezierLine(
|
||||
new Point(60.000, 25.000, Point.CARTESIAN),
|
||||
new Point(10.000, 25.000, Point.CARTESIAN)
|
||||
)
|
||||
)
|
||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
||||
.build();
|
||||
|
||||
follower.followPath(path);
|
||||
|
||||
|
||||
@ -71,8 +71,9 @@ measurements will be in centimeters.
|
||||
of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but
|
||||
what works best for you is most important. Higher numbers will cause a faster brake, but increase
|
||||
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
|
||||
* sensitive than the others. For reference,
|
||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`.
|
||||
|
||||
* The drive PID is much, much more sensitive than the others. For reference,
|
||||
my P values were in the hundredths and thousandths place values, and my D values were in the hundred
|
||||
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
|
||||
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
||||
|
||||
@ -81,7 +81,7 @@ public class FollowerConstants {
|
||||
// Kalman filter parameters for the drive error Kalman filter
|
||||
public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
|
||||
6,
|
||||
1);
|
||||
3);
|
||||
|
||||
|
||||
// Mass of robot in kilograms
|
||||
|
||||
Reference in New Issue
Block a user