Compare commits
33 Commits
branch-rc-
...
047d0fa3c3
| Author | SHA1 | Date | |
|---|---|---|---|
| 047d0fa3c3 | |||
| c63319f9c4 | |||
| c824580b33 | |||
| 883906885b | |||
| 5595fcccd4 | |||
| 3e79d86443 | |||
| c9ffd4f061 | |||
| 021dfa7222 | |||
| 233b177cf6 | |||
| 0ab402af0f | |||
| aa496b8237 | |||
| 66f3339e26 | |||
| ad0a8d3374 | |||
| dc71eb4317 | |||
| 94144780b8 | |||
| a362d2e004 | |||
| 2008c3cd88 | |||
| c5be3cd932 | |||
| 7d83b9c254 | |||
| 5f50d053c5 | |||
| 0cfb57c643 | |||
| 50db1f9175 | |||
| 552bb3e25a | |||
| c1076a832c | |||
| 7bf8b0c357 | |||
| adfab3e8af | |||
| 0f7ea50907 | |||
| 239f168540 | |||
| f2bcdcc55a | |||
| e08aac773d | |||
| 3950a83ac1 | |||
| 552ff3f339 | |||
| 4eef485dab |
@ -1,135 +0,0 @@
|
|||||||
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 = "AsherPathV1", group = "Autonomous Pathing Tuning")
|
|
||||||
public class AsherPathV1 extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(10.0, 40, 90);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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(.4);
|
|
||||||
|
|
||||||
follower.setStartingPose(startPose);
|
|
||||||
|
|
||||||
path = follower.pathBuilder()
|
|
||||||
/*
|
|
||||||
* Only update this path
|
|
||||||
*/
|
|
||||||
.addPath(
|
|
||||||
// Line 1
|
|
||||||
new BezierCurve(
|
|
||||||
new Point(9.757, 84.983, Point.CARTESIAN),
|
|
||||||
new Point(33.000, 105.000, Point.CARTESIAN),
|
|
||||||
new Point(80.000, 118.000, Point.CARTESIAN),
|
|
||||||
new Point(55.000, 120.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.addPath(
|
|
||||||
// Line 2
|
|
||||||
new BezierCurve(
|
|
||||||
new Point(55.000, 120.000, Point.CARTESIAN),
|
|
||||||
new Point(22.000, 106.000, Point.CARTESIAN),
|
|
||||||
new Point(11.000, 131.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.addPath(
|
|
||||||
// Line 3
|
|
||||||
new BezierCurve(
|
|
||||||
new Point(11.000, 131.000, Point.CARTESIAN),
|
|
||||||
new Point(75.000, 95.000, Point.CARTESIAN),
|
|
||||||
new Point(112.000, 132.000, Point.CARTESIAN),
|
|
||||||
new Point(61.000, 131.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.addPath(
|
|
||||||
// Line 4
|
|
||||||
new BezierLine(
|
|
||||||
new Point(61.000, 131.000, Point.CARTESIAN),
|
|
||||||
new Point(11.000, 131.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.addPath(
|
|
||||||
// Line 5
|
|
||||||
new BezierCurve(
|
|
||||||
new Point(11.000, 131.000, Point.CARTESIAN),
|
|
||||||
new Point(100.000, 118.000, Point.CARTESIAN),
|
|
||||||
new Point(103.000, 135.000, Point.CARTESIAN),
|
|
||||||
new Point(61.000, 135.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.addPath(
|
|
||||||
// Line 6
|
|
||||||
new BezierLine(
|
|
||||||
new Point(61.000, 135.000, Point.CARTESIAN),
|
|
||||||
new Point(11.000, 131.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.addPath(
|
|
||||||
// Line 7
|
|
||||||
new BezierCurve(
|
|
||||||
new Point(11.000, 131.000, Point.CARTESIAN),
|
|
||||||
new Point(113.000, 95.000, Point.CARTESIAN),
|
|
||||||
new Point(67.000, 95.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
|
|
||||||
/*
|
|
||||||
* End of only update this path
|
|
||||||
*/
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,87 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode;
|
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
|
||||||
|
|
||||||
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 com.qualcomm.robotcore.hardware.DcMotor;
|
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
|
|
||||||
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 - Straight Path", group = "Autonomous Pathing Tuning")
|
|
||||||
public class AutoExample extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(0.0, 20.0, 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(.6);
|
|
||||||
|
|
||||||
follower.setStartingPose(startPose);
|
|
||||||
|
|
||||||
path = follower.pathBuilder()
|
|
||||||
.addPath(
|
|
||||||
// Line 1
|
|
||||||
new BezierLine(
|
|
||||||
new Point(0.000, 20.000, Point.CARTESIAN),
|
|
||||||
new Point(50.000, 20.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,106 +0,0 @@
|
|||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,142 +0,0 @@
|
|||||||
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.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 = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
|
|
||||||
public class AutoExampleSeason2025V1 extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(15.0, 35, 90);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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(.375);
|
|
||||||
|
|
||||||
follower.setStartingPose(startPose);
|
|
||||||
|
|
||||||
path = follower.pathBuilder()
|
|
||||||
.addPath(
|
|
||||||
// Line 1
|
|
||||||
new BezierLine(
|
|
||||||
new Point(15.000, 35.000, Point.CARTESIAN),
|
|
||||||
new Point(60.000, 35.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 2
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.000, 35.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(15.000, 25.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 4
|
|
||||||
new BezierLine(
|
|
||||||
new Point(15.000, 25.000, Point.CARTESIAN),
|
|
||||||
new Point(60.000, 25.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 5
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.000, 25.000, Point.CARTESIAN),
|
|
||||||
new Point(60.000, 15.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 6
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.000, 15.000, Point.CARTESIAN),
|
|
||||||
new Point(15.000, 15.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 7
|
|
||||||
new BezierLine(
|
|
||||||
new Point(15.000, 15.000, Point.CARTESIAN),
|
|
||||||
new Point(60.000, 15.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 8
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.000, 15.000, Point.CARTESIAN),
|
|
||||||
new Point(60.000, 8.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 9
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.000, 8.000, Point.CARTESIAN),
|
|
||||||
new Point(15.000, 8.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90)).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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,89 +0,0 @@
|
|||||||
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 - Curve and Line", group = "Autonomous Pathing Tuning")
|
|
||||||
public class AutoExampleThree extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(10,45, 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(.4);
|
|
||||||
|
|
||||||
follower.setStartingPose(startPose);
|
|
||||||
|
|
||||||
path = follower.pathBuilder()
|
|
||||||
.addPath(
|
|
||||||
// Line 1
|
|
||||||
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)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
|
||||||
.addPath(
|
|
||||||
// Line 2
|
|
||||||
new BezierLine(
|
|
||||||
new Point(50.000, 20.000, Point.CARTESIAN),
|
|
||||||
new Point(10.000, 20.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90))
|
|
||||||
.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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,81 +0,0 @@
|
|||||||
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 - Simple Curve", group = "Autonomous Pathing Tuning")
|
|
||||||
public class AutoExampleTwo extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(10.0, 45, 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(.4);
|
|
||||||
|
|
||||||
follower.setStartingPose(startPose);
|
|
||||||
|
|
||||||
path = follower.pathBuilder()
|
|
||||||
.addPath(
|
|
||||||
// Line 1
|
|
||||||
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)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90))
|
|
||||||
.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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,247 +0,0 @@
|
|||||||
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 = "BluebAutoV1", group = "Autonomous Pathing Tuning")
|
|
||||||
public class BluebAutoV1 extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(7.5, 72, 90);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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(.4);
|
|
||||||
|
|
||||||
follower.setStartingPose(startPose);
|
|
||||||
|
|
||||||
path = follower.pathBuilder()
|
|
||||||
.addPath(
|
|
||||||
// Line 1
|
|
||||||
new BezierLine(
|
|
||||||
new Point(7.5, 72, Point.CARTESIAN),
|
|
||||||
new Point(29.893, 38.250, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 2
|
|
||||||
new BezierLine(
|
|
||||||
new Point(29.893, 38.250, Point.CARTESIAN),
|
|
||||||
new Point(65.250, 32.143, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 3
|
|
||||||
new BezierLine(
|
|
||||||
new Point(65.250, 32.143, Point.CARTESIAN),
|
|
||||||
new Point(61.714, 24.429, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 4
|
|
||||||
new BezierLine(
|
|
||||||
new Point(61.714, 24.429, Point.CARTESIAN),
|
|
||||||
new Point(13.821, 22.821, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 5
|
|
||||||
new BezierLine(
|
|
||||||
new Point(13.821, 22.821, Point.CARTESIAN),
|
|
||||||
new Point(61.714, 24.429, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 6
|
|
||||||
new BezierLine(
|
|
||||||
new Point(61.714, 24.429, Point.CARTESIAN),
|
|
||||||
new Point(60.750, 12.696, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 7
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.750, 12.696, Point.CARTESIAN),
|
|
||||||
new Point(12.375, 13.179, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 8
|
|
||||||
new BezierLine(
|
|
||||||
new Point(12.375, 13.179, Point.CARTESIAN),
|
|
||||||
new Point(60.750, 12.536, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 9
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.750, 12.536, Point.CARTESIAN),
|
|
||||||
new Point(60.589, 9.321, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 10
|
|
||||||
new BezierLine(
|
|
||||||
new Point(60.589, 9.321, Point.CARTESIAN),
|
|
||||||
new Point(12.536, 8.357, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 11
|
|
||||||
new BezierLine(
|
|
||||||
new Point(12.536, 8.357, Point.CARTESIAN),
|
|
||||||
new Point(26.679, 8.679, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 12
|
|
||||||
new BezierLine(
|
|
||||||
new Point(26.679, 8.679, Point.CARTESIAN),
|
|
||||||
new Point(22.821, 109.446, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 13
|
|
||||||
new BezierLine(
|
|
||||||
new Point(22.821, 109.446, Point.CARTESIAN),
|
|
||||||
new Point(70.714, 109.446, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 14
|
|
||||||
new BezierLine(
|
|
||||||
new Point(70.714, 109.446, Point.CARTESIAN),
|
|
||||||
new Point(71.036, 120.214, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 15
|
|
||||||
new BezierLine(
|
|
||||||
new Point(71.036, 120.214, Point.CARTESIAN),
|
|
||||||
new Point(22.179, 120.214, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 16
|
|
||||||
new BezierLine(
|
|
||||||
new Point(22.179, 120.214, Point.CARTESIAN),
|
|
||||||
new Point(11.089, 130.821, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 17
|
|
||||||
new BezierLine(
|
|
||||||
new Point(11.089, 130.821, Point.CARTESIAN),
|
|
||||||
new Point(70.714, 112.018, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 18
|
|
||||||
new BezierLine(
|
|
||||||
new Point(70.714, 112.018, Point.CARTESIAN),
|
|
||||||
new Point(70.714, 128.250, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 19
|
|
||||||
new BezierLine(
|
|
||||||
new Point(70.714, 128.250, Point.CARTESIAN),
|
|
||||||
new Point(9.964, 130.018, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 20
|
|
||||||
new BezierLine(
|
|
||||||
new Point(9.964, 130.018, Point.CARTESIAN),
|
|
||||||
new Point(70.554, 130.500, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 21
|
|
||||||
new BezierLine(
|
|
||||||
new Point(70.554, 130.500, Point.CARTESIAN),
|
|
||||||
new Point(70.393, 135.000, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90))
|
|
||||||
.addPath(
|
|
||||||
// Line 22
|
|
||||||
new BezierLine(
|
|
||||||
new Point(70.393, 135.000, Point.CARTESIAN),
|
|
||||||
new Point(13.821, 134.839, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(90)).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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode;
|
|
||||||
|
|
||||||
public class BluenbAutov1 {
|
|
||||||
}
|
|
||||||
@ -0,0 +1,24 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
|
||||||
|
|
||||||
|
@TeleOp(name = "CometBot Auto v2", group = "Development")
|
||||||
|
public class CometBotDevAuto extends OpMode {
|
||||||
|
|
||||||
|
public CometBotAutoDevelopment runMode;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2);
|
||||||
|
runMode.init();
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
runMode.update();
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -10,14 +10,12 @@ public class PedroConstants {
|
|||||||
/*
|
/*
|
||||||
Robot parameters
|
Robot parameters
|
||||||
*/
|
*/
|
||||||
// Turn localizer - -0.003
|
|
||||||
|
|
||||||
|
|
||||||
// Robot motor configurations
|
// Robot motor configurations
|
||||||
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
|
public static final String FRONT_LEFT_MOTOR = "front-left";
|
||||||
public static final String BACK_LEFT_MOTOR = "Drive back lt";
|
public static final String BACK_LEFT_MOTOR = "back-left";
|
||||||
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
|
public static final String FRONT_RIGHT_MOTOR = "front-right";
|
||||||
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
|
public static final String BACK_RIGHT_MOTOR = "back-right";
|
||||||
|
|
||||||
// Robot motor direction
|
// Robot motor direction
|
||||||
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
|
public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
|
||||||
@ -25,46 +23,60 @@ public class PedroConstants {
|
|||||||
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
|
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
|
||||||
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
|
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
|
||||||
|
|
||||||
|
/*
|
||||||
|
Motor Max Power
|
||||||
|
*/
|
||||||
|
public static final double MAX_POWER = .75;
|
||||||
|
|
||||||
// Robot IMU configuration
|
// Robot IMU configuration
|
||||||
public static final String IMU = "imu";
|
public static final String IMU = "imu";
|
||||||
|
|
||||||
// Robot IMU placement
|
// Robot IMU placement
|
||||||
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
|
public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
|
||||||
= RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
|
= RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
|
||||||
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
|
public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
|
||||||
= RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
|
= RevHubOrientationOnRobot.UsbFacingDirection.UP;
|
||||||
|
|
||||||
// Robot encoders
|
// Robot encoders
|
||||||
public static final String LEFT_ENCODER = "encoder left";
|
// NOTE: Encoders are plugged into the same ports as motors hence the weird names
|
||||||
public static final String RIGHT_ENCODER = "encoder right";
|
public static final String RIGHT_ENCODER = "front-left"; //2
|
||||||
public static final String BACK_ENCODER = "encoder back";
|
public static final String BACK_ENCODER = "front-right"; //1
|
||||||
|
public static final String LEFT_ENCODER = "back-right"; //0
|
||||||
|
|
||||||
// Robot encoder direction
|
// Robot encoder direction
|
||||||
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
|
public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||||
public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
|
public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
|
||||||
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
|
public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
|
||||||
|
|
||||||
|
// Arm config
|
||||||
|
public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
|
||||||
|
public static final String LIFT_SLIDE_RIGHT_MOTOR = "lift-slide-right";
|
||||||
|
public static final String CLAW_SERVO = "claw-servo";
|
||||||
|
public static final String WRIST_SERVO = "wrist-servo";
|
||||||
|
public static final String ARM_SERVO = "arm-servo";
|
||||||
|
public static final String THUMB_SERVO = "thumb-servo";
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Pedro's parameters
|
Pedro's parameters
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// The weight of the robot in Kilograms
|
// The weight of the robot in Kilograms
|
||||||
public static final double ROBOT_WEIGHT_IN_KG = 10.5;
|
public static final double ROBOT_WEIGHT_IN_KG = 9;
|
||||||
|
|
||||||
// Maximum velocity of the robot going forward
|
// Maximum velocity of the robot going forward
|
||||||
public static final double ROBOT_SPEED_FORWARD = 51.4598;
|
public static final double ROBOT_SPEED_FORWARD = 53.223;
|
||||||
|
|
||||||
// Maximum velocity of the robot going right
|
// Maximum velocity of the robot going right
|
||||||
public static final double ROBOT_SPEED_LATERAL = 28.7119;
|
public static final double ROBOT_SPEED_LATERAL = 41.4081;
|
||||||
|
|
||||||
// Rate of deceleration when power is cut-off when the robot is moving forward
|
// Rate of deceleration when power is cut-off when the robot is moving forward
|
||||||
public static final double FORWARD_ZERO_POWER_ACCEL = -59.805;
|
public static final double FORWARD_ZERO_POWER_ACCEL = -76.8421;
|
||||||
|
|
||||||
// Rate of deceleration when power is cut-off when the robot is moving to the right
|
// Rate of deceleration when power is cut-off when the robot is moving to the right
|
||||||
public static final double LATERAL_ZERO_POWER_ACCEL = -99.672;
|
public static final double LATERAL_ZERO_POWER_ACCEL = -93.4183;
|
||||||
|
|
||||||
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
|
// Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
|
||||||
public static final double ZERO_POWER_ACCEL_MULT = 3.5;
|
public static final double ZERO_POWER_ACCEL_MULT = 4.0;
|
||||||
|
|
||||||
/* Centripetal force correction - increase if robot is correcting into the path
|
/* Centripetal force correction - increase if robot is correcting into the path
|
||||||
- decrease if robot is correcting away from the path */
|
- decrease if robot is correcting away from the path */
|
||||||
|
|||||||
@ -1,12 +1,9 @@
|
|||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode;
|
||||||
import static android.os.SystemClock.sleep;
|
|
||||||
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
|
||||||
import com.acmerobotics.dashboard.FtcDashboard;
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
import com.acmerobotics.dashboard.config.Config;
|
|
||||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
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.robotcore.external.Telemetry;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||||
@ -16,32 +13,16 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
|
|||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||||
|
|
||||||
/**
|
@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
|
||||||
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
public class PreLoadedBlueBasketAuto extends OpMode {
|
||||||
* 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 = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
|
|
||||||
public class BlueBasketAuto extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
private Follower follower;
|
private Follower follower;
|
||||||
|
|
||||||
private PathChain path;
|
private PathChain path;
|
||||||
|
|
||||||
private final Pose startPose = new Pose(11.25, 95.75);
|
private final Pose startPose = new Pose(7.875, 89.357);
|
||||||
|
|
||||||
/**
|
|
||||||
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
|
||||||
* initializes the FTC Dashboard telemetry.
|
|
||||||
*/
|
|
||||||
@Override
|
@Override
|
||||||
public void init() {
|
public void init() {
|
||||||
follower = new Follower(hardwareMap);
|
follower = new Follower(hardwareMap);
|
||||||
@ -54,75 +35,76 @@ public class BlueBasketAuto extends OpMode {
|
|||||||
.addPath(
|
.addPath(
|
||||||
// Line 1
|
// Line 1
|
||||||
new BezierLine(
|
new BezierLine(
|
||||||
new Point(11.250, 95.750, Point.CARTESIAN),
|
new Point(8.036, 89.196, Point.CARTESIAN),
|
||||||
new Point(37.000, 108.000, Point.CARTESIAN)
|
new Point(10.125, 126.804, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 2
|
// Line 2
|
||||||
new BezierCurve(
|
new BezierCurve(
|
||||||
new Point(37.000, 108.000, Point.CARTESIAN),
|
new Point(10.125, 126.804, Point.CARTESIAN),
|
||||||
new Point(73.286, 111.536, Point.CARTESIAN),
|
new Point(37.607, 90.000, Point.CARTESIAN),
|
||||||
new Point(67.821, 120.536, Point.CARTESIAN)
|
new Point(62.357, 119.893, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 3
|
// Line 3
|
||||||
new BezierLine(
|
new BezierCurve(
|
||||||
new Point(67.821, 120.536, Point.CARTESIAN),
|
new Point(62.357, 119.893, Point.CARTESIAN),
|
||||||
new Point(28.000, 121.500, Point.CARTESIAN)
|
new Point(33.750, 112.500, Point.CARTESIAN),
|
||||||
|
new Point(15.107, 130.661, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 4
|
// Line 4
|
||||||
new BezierLine(
|
new BezierCurve(
|
||||||
new Point(28.000, 121.500, Point.CARTESIAN),
|
new Point(15.107, 130.661, Point.CARTESIAN),
|
||||||
new Point(15.750, 126.321, Point.CARTESIAN)
|
new Point(58.821, 103.018, Point.CARTESIAN),
|
||||||
|
new Point(59.625, 126.964, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 5
|
// Line 5
|
||||||
new BezierCurve(
|
new BezierLine(
|
||||||
new Point(15.750, 126.321, Point.CARTESIAN),
|
new Point(59.625, 126.964, Point.CARTESIAN),
|
||||||
new Point(59.000, 102.500, Point.CARTESIAN),
|
new Point(15.107, 130.339, Point.CARTESIAN)
|
||||||
new Point(68.700, 130.500, Point.CARTESIAN)
|
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 6
|
// Line 6
|
||||||
new BezierLine(
|
new BezierLine(
|
||||||
new Point(68.700, 130.500, Point.CARTESIAN),
|
new Point(15.107, 130.339, Point.CARTESIAN),
|
||||||
new Point(22.661, 130.821, Point.CARTESIAN)
|
new Point(59.625, 126.964, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 7
|
// Line 7
|
||||||
new BezierCurve(
|
new BezierLine(
|
||||||
new Point(22.661, 130.821, Point.CARTESIAN),
|
new Point(59.625, 126.964, Point.CARTESIAN),
|
||||||
new Point(49.018, 121.179, Point.CARTESIAN),
|
new Point(57.857, 133.071, Point.CARTESIAN)
|
||||||
new Point(63.804, 135.321, Point.CARTESIAN)
|
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 8
|
// Line 8
|
||||||
new BezierLine(
|
new BezierLine(
|
||||||
new Point(63.804, 135.321, Point.CARTESIAN),
|
new Point(57.857, 133.071, Point.CARTESIAN),
|
||||||
new Point(26.679, 135.964, Point.CARTESIAN)
|
new Point(18.964, 134.679, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
.addPath(
|
.addPath(
|
||||||
// Line 9
|
// Line 9
|
||||||
new BezierLine(
|
new BezierCurve(
|
||||||
new Point(26.679, 135.964, Point.CARTESIAN),
|
new Point(18.964, 134.679, Point.CARTESIAN),
|
||||||
new Point(83.250, 95.464, Point.CARTESIAN)
|
new Point(84.536, 131.786, Point.CARTESIAN),
|
||||||
|
new Point(80.036, 96.429, Point.CARTESIAN)
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
|
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
|
||||||
@ -132,16 +114,9 @@ public class BlueBasketAuto extends OpMode {
|
|||||||
telemetryA.update();
|
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
|
@Override
|
||||||
public void loop() {
|
public void loop() {
|
||||||
follower.update();
|
follower.update();
|
||||||
if (follower.atParametricEnd()) {
|
|
||||||
follower.followPath(path);
|
|
||||||
}
|
|
||||||
follower.telemetryDebug(telemetryA);
|
follower.telemetryDebug(telemetryA);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -1,161 +0,0 @@
|
|||||||
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 = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
|
|
||||||
public class RedBasketAuto extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(
|
|
||||||
135.6
|
|
||||||
, 48.05);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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 BezierLine(
|
|
||||||
new Point(136.286, 48.536, Point.CARTESIAN),
|
|
||||||
new Point(102.375, 32.946, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 2
|
|
||||||
new BezierLine(
|
|
||||||
new Point(102.375, 32.946, Point.CARTESIAN),
|
|
||||||
new Point(80.679, 31.982, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 3
|
|
||||||
new BezierLine(
|
|
||||||
new Point(80.679, 31.982, Point.CARTESIAN),
|
|
||||||
new Point(80.679, 23.304, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 4
|
|
||||||
new BezierLine(
|
|
||||||
new Point(80.679, 23.304, Point.CARTESIAN),
|
|
||||||
new Point(131.143, 21.536, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 5
|
|
||||||
new BezierLine(
|
|
||||||
new Point(131.143, 21.536, Point.CARTESIAN),
|
|
||||||
new Point(81.000, 23.304, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 6
|
|
||||||
new BezierLine(
|
|
||||||
new Point(81.000, 23.304, Point.CARTESIAN),
|
|
||||||
new Point(81.161, 13.179, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 7
|
|
||||||
new BezierLine(
|
|
||||||
new Point(81.161, 13.179, Point.CARTESIAN),
|
|
||||||
new Point(124.554, 13.018, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 8
|
|
||||||
new BezierLine(
|
|
||||||
new Point(124.554, 13.018, Point.CARTESIAN),
|
|
||||||
new Point(81.000, 13.018, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 9
|
|
||||||
new BezierLine(
|
|
||||||
new Point(81.000, 13.018, Point.CARTESIAN),
|
|
||||||
new Point(80.839, 8.800, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 10
|
|
||||||
new BezierLine(
|
|
||||||
new Point(80.839, 8.800, Point.CARTESIAN),
|
|
||||||
new Point(120.054, 8.518, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(180))
|
|
||||||
.addPath(
|
|
||||||
// Line 11
|
|
||||||
new BezierCurve(
|
|
||||||
new Point(120.054, 8.518, Point.CARTESIAN),
|
|
||||||
new Point(49.821, 27.643, Point.CARTESIAN),
|
|
||||||
new Point(60.429, 48.214, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(90)).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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,171 +0,0 @@
|
|||||||
/* Copyright (c) 2022 FIRST. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without modification,
|
|
||||||
* are permitted (subject to the limitations in the disclaimer below) provided that
|
|
||||||
* the following conditions are met:
|
|
||||||
*
|
|
||||||
* Redistributions of source code must retain the above copyright notice, this list
|
|
||||||
* of conditions and the following disclaimer.
|
|
||||||
*
|
|
||||||
* Redistributions in binary form must reproduce the above copyright notice, this
|
|
||||||
* list of conditions and the following disclaimer in the documentation and/or
|
|
||||||
* other materials provided with the distribution.
|
|
||||||
*
|
|
||||||
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
|
||||||
* promote products derived from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
|
||||||
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
|
||||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
||||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
|
||||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
||||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
||||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
||||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
||||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*/
|
|
||||||
|
|
||||||
package org.firstinspires.ftc.teamcode;
|
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_LOGO_FACING_DIRECTION;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_DIRECTION;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
|
||||||
|
|
||||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
import com.qualcomm.robotcore.hardware.IMU;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* This OpMode shows how to use the new universal IMU interface. This
|
|
||||||
* interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
|
|
||||||
* on the robot with the name "imu".
|
|
||||||
*
|
|
||||||
* The sample will display the current Yaw, Pitch and Roll of the robot.<br>
|
|
||||||
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
|
|
||||||
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
|
|
||||||
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
|
|
||||||
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
|
|
||||||
*
|
|
||||||
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
|
|
||||||
*
|
|
||||||
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
|
|
||||||
* (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
|
|
||||||
*
|
|
||||||
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
|
|
||||||
* the alternative SensorIMUNonOrthogonal sample in this folder.
|
|
||||||
*
|
|
||||||
* This "Orthogonal" requirement means that:
|
|
||||||
*
|
|
||||||
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
|
|
||||||
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
|
|
||||||
*
|
|
||||||
* 2) The USB ports can only be pointing in one of the same six directions:<br>
|
|
||||||
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
|
|
||||||
*
|
|
||||||
* So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
|
|
||||||
* logoFacingDirection<br>
|
|
||||||
* usbFacingDirection
|
|
||||||
*
|
|
||||||
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
|
|
||||||
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
|
|
||||||
*
|
|
||||||
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
|
|
||||||
* to use those parameters.
|
|
||||||
*/
|
|
||||||
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
|
|
||||||
@Disabled // Comment this out to add to the OpMode list
|
|
||||||
public class SensorIMUOrthogonal extends LinearOpMode {
|
|
||||||
// The IMU sensor object
|
|
||||||
IMU imu;
|
|
||||||
private Encoder leftEncoder;
|
|
||||||
private Encoder rightEncoder;
|
|
||||||
private Encoder strafeEncoder;
|
|
||||||
|
|
||||||
//----------------------------------------------------------------------------------------------
|
|
||||||
// Main logic
|
|
||||||
//----------------------------------------------------------------------------------------------
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void runOpMode() throws InterruptedException {
|
|
||||||
|
|
||||||
// Retrieve and initialize the IMU.
|
|
||||||
// This sample expects the IMU to be in a REV Hub and named "imu".
|
|
||||||
imu = hardwareMap.get(IMU.class, PedroConstants.IMU);
|
|
||||||
|
|
||||||
// TODO: replace these with your encoder ports
|
|
||||||
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
|
||||||
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
|
|
||||||
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
|
|
||||||
|
|
||||||
// TODO: reverse any encoders necessary
|
|
||||||
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
|
|
||||||
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
|
|
||||||
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
|
|
||||||
|
|
||||||
/* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
|
|
||||||
*
|
|
||||||
* Two input parameters are required to fully specify the Orientation.
|
|
||||||
* The first parameter specifies the direction the printed logo on the Hub is pointing.
|
|
||||||
* The second parameter specifies the direction the USB connector on the Hub is pointing.
|
|
||||||
* All directions are relative to the robot, and left/right is as-viewed from behind the robot.
|
|
||||||
*
|
|
||||||
* If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
|
|
||||||
* RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* The next two lines define Hub orientation.
|
|
||||||
* The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
|
|
||||||
*
|
|
||||||
* To Do: EDIT these two lines to match YOUR mounting configuration.
|
|
||||||
*/
|
|
||||||
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION;
|
|
||||||
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION;
|
|
||||||
|
|
||||||
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
|
|
||||||
|
|
||||||
// Now initialize the IMU with this mounting orientation
|
|
||||||
// Note: if you choose two conflicting directions, this initialization will cause a code exception.
|
|
||||||
imu.initialize(new IMU.Parameters(orientationOnRobot));
|
|
||||||
|
|
||||||
// Loop and update the dashboard
|
|
||||||
while (!isStopRequested()) {
|
|
||||||
|
|
||||||
telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
|
|
||||||
|
|
||||||
// Check to see if heading reset is requested
|
|
||||||
if (gamepad1.y) {
|
|
||||||
telemetry.addData("Yaw", "Resetting\n");
|
|
||||||
imu.resetYaw();
|
|
||||||
} else {
|
|
||||||
telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
// Retrieve Rotational Angles and Velocities
|
|
||||||
YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
|
|
||||||
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
|
|
||||||
|
|
||||||
telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
|
|
||||||
telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
|
|
||||||
telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
|
|
||||||
telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
|
|
||||||
telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
|
|
||||||
telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
|
|
||||||
telemetry.update();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -0,0 +1,86 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.cometbots;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||||
|
import com.acmerobotics.roadrunner.Action;
|
||||||
|
import com.acmerobotics.roadrunner.SequentialAction;
|
||||||
|
import com.acmerobotics.roadrunner.SleepAction;
|
||||||
|
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||||
|
import org.firstinspires.ftc.teamcode.states.FieldStates;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
|
||||||
|
|
||||||
|
public class CometBotAutoDevelopment {
|
||||||
|
|
||||||
|
/*
|
||||||
|
Subsystems
|
||||||
|
*/
|
||||||
|
private DualMotorSliderSubsystem dualSlides;
|
||||||
|
|
||||||
|
/*
|
||||||
|
Controllers
|
||||||
|
*/
|
||||||
|
public Gamepad gamepad1;
|
||||||
|
public Gamepad gamepad2;
|
||||||
|
public Gamepad currentGamepad1;
|
||||||
|
public Gamepad currentGamepad2;
|
||||||
|
public Gamepad previousGamepad1;
|
||||||
|
public Gamepad previousGamepad2;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
|
||||||
|
dualSlides = new DualMotorSliderSubsystem(hardwareMap);
|
||||||
|
this.gamepad1 = gamepad1;
|
||||||
|
this.gamepad2 = gamepad2;
|
||||||
|
currentGamepad1 = new Gamepad();
|
||||||
|
currentGamepad2 = new Gamepad();
|
||||||
|
previousGamepad1 = new Gamepad();
|
||||||
|
previousGamepad2 = new Gamepad();
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void init() {
|
||||||
|
dualSlides.init();
|
||||||
|
follower.setMaxPower(MAX_POWER);
|
||||||
|
follower.startTeleopDrive();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void update() {
|
||||||
|
previousGamepad1.copy(currentGamepad1);
|
||||||
|
currentGamepad1.copy(gamepad1);
|
||||||
|
previousGamepad2.copy(currentGamepad2);
|
||||||
|
currentGamepad2.copy(gamepad2);
|
||||||
|
|
||||||
|
/*
|
||||||
|
Check if dpad_up/down is being pressed for slides
|
||||||
|
*/
|
||||||
|
dualSlides.update();
|
||||||
|
dualSlidesToLowBucketPosition();
|
||||||
|
dualSlidesToHighBucketPosition();
|
||||||
|
|
||||||
|
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
|
||||||
|
follower.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
private void dualSlidesToHighBucketPosition() {
|
||||||
|
if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
|
||||||
|
dualSlides.toHighBucketPosition();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void dualSlidesToLowBucketPosition() {
|
||||||
|
if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
|
||||||
|
dualSlides.toLowBucketPosition();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -27,7 +27,7 @@
|
|||||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode.cometbots.projects;
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||||
@ -44,7 +44,6 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTI
|
|||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
||||||
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
@ -0,0 +1,72 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.cometbots.tests;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
|
||||||
|
@Config
|
||||||
|
@Autonomous(name = "Lift Motor Subsystem - PID Test")
|
||||||
|
public class DualMotorSliderTest extends LinearOpMode {
|
||||||
|
|
||||||
|
private DcMotorEx liftSlideLeft;
|
||||||
|
private DcMotorEx liftSlideRight;
|
||||||
|
public static double kp = 0.0015, ki = 0, kd = 0;
|
||||||
|
private double lastError = 0;
|
||||||
|
private double integralSum = 0;
|
||||||
|
public static int targetPosition = 0;
|
||||||
|
private final FtcDashboard dashboard = FtcDashboard.getInstance();
|
||||||
|
private ElapsedTime timer = new ElapsedTime();
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
TelemetryPacket packet = new TelemetryPacket();
|
||||||
|
dashboard.setTelemetryTransmissionInterval(25);
|
||||||
|
|
||||||
|
liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
|
||||||
|
liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
|
||||||
|
|
||||||
|
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
while(opModeIsActive()) {
|
||||||
|
double power = calculatePower(targetPosition, liftSlideLeft.getCurrentPosition());
|
||||||
|
|
||||||
|
packet.put("Power", power);
|
||||||
|
packet.put("Position", liftSlideLeft.getCurrentPosition());
|
||||||
|
packet.put("Error", lastError);
|
||||||
|
packet.put("Seconds", timer.seconds());
|
||||||
|
|
||||||
|
liftSlideLeft.setPower(power);
|
||||||
|
liftSlideRight.setPower(power);
|
||||||
|
|
||||||
|
dashboard.sendTelemetryPacket(packet);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private double calculatePower(int targetPosition, int currentPosition) {
|
||||||
|
// reference is targetPosition, state is currentPosition
|
||||||
|
double error = targetPosition - currentPosition;
|
||||||
|
integralSum += error * timer.seconds();
|
||||||
|
double derivative = (error - lastError) / timer.seconds();
|
||||||
|
lastError = error;
|
||||||
|
timer.reset();
|
||||||
|
return (error * kp) + (derivative * kd) + (integralSum * ki);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -71,9 +71,8 @@ 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
|
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
|
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
|
oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in
|
||||||
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`.
|
`FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more
|
||||||
|
* sensitive than the others. For reference,
|
||||||
* 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
|
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
|
thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and
|
||||||
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
`useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth`
|
||||||
|
|||||||
@ -5,10 +5,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
|
|||||||
import com.qualcomm.robotcore.hardware.IMU;
|
import com.qualcomm.robotcore.hardware.IMU;
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.DriveEncoderLocalizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
|
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
||||||
|
|
||||||
@ -71,7 +68,6 @@ public class PoseUpdater {
|
|||||||
public PoseUpdater(HardwareMap hardwareMap) {
|
public PoseUpdater(HardwareMap hardwareMap) {
|
||||||
// TODO: replace the second argument with your preferred localizer
|
// TODO: replace the second argument with your preferred localizer
|
||||||
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
|
this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
|
||||||
// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@ -1,272 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
|
||||||
|
|
||||||
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 com.acmerobotics.dashboard.config.Config;
|
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a
|
|
||||||
* localizer that uses the drive encoder set up.
|
|
||||||
*
|
|
||||||
* @author Anyi Lin - 10158 Scott's Bots
|
|
||||||
* @version 1.0, 4/2/2024
|
|
||||||
*/
|
|
||||||
@Config
|
|
||||||
public class DriveEncoderLocalizer extends Localizer {
|
|
||||||
private HardwareMap hardwareMap;
|
|
||||||
private Pose startPose;
|
|
||||||
private Pose displacementPose;
|
|
||||||
private Pose currentVelocity;
|
|
||||||
private Matrix prevRotationMatrix;
|
|
||||||
private NanoTimer timer;
|
|
||||||
private long deltaTimeNano;
|
|
||||||
private Encoder leftFront;
|
|
||||||
private Encoder rightFront;
|
|
||||||
private Encoder leftRear;
|
|
||||||
private Encoder rightRear;
|
|
||||||
private double totalHeading;
|
|
||||||
public static double FORWARD_TICKS_TO_INCHES = -0.6308;
|
|
||||||
public static double STRAFE_TICKS_TO_INCHES = 46.4839;
|
|
||||||
public static double TURN_TICKS_TO_RADIANS = -0.002;
|
|
||||||
public static double ROBOT_WIDTH = 1;
|
|
||||||
public static double ROBOT_LENGTH = 1;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
|
||||||
* facing 0 heading.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
*/
|
|
||||||
public DriveEncoderLocalizer(HardwareMap map) {
|
|
||||||
this(map, new Pose());
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose
|
|
||||||
* specifying the starting pose of the localizer.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
* @param setStartPose the Pose to start from
|
|
||||||
*/
|
|
||||||
public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) {
|
|
||||||
hardwareMap = map;
|
|
||||||
|
|
||||||
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName));
|
|
||||||
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName));
|
|
||||||
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName));
|
|
||||||
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName));
|
|
||||||
|
|
||||||
// TODO: reverse any encoders necessary
|
|
||||||
leftFront.setDirection(Encoder.REVERSE);
|
|
||||||
rightRear.setDirection(Encoder.REVERSE);
|
|
||||||
leftRear.setDirection(Encoder.FORWARD);
|
|
||||||
rightRear.setDirection(Encoder.FORWARD);
|
|
||||||
|
|
||||||
setStartPose(setStartPose);
|
|
||||||
timer = new NanoTimer();
|
|
||||||
deltaTimeNano = 1;
|
|
||||||
displacementPose = new Pose();
|
|
||||||
currentVelocity = new Pose();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current pose estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current pose estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getPose() {
|
|
||||||
return MathFunctions.addPoses(startPose, displacementPose);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getVelocity() {
|
|
||||||
return currentVelocity.copy();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Vector
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Vector getVelocityVector() {
|
|
||||||
return currentVelocity.getVector();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the start pose. Changing the start pose should move the robot as if all its
|
|
||||||
* previous movements were displacing it from its new start pose.
|
|
||||||
*
|
|
||||||
* @param setStart the new start pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setStartPose(Pose setStart) {
|
|
||||||
startPose = setStart;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the Matrix that contains the previous pose's heading rotation.
|
|
||||||
*
|
|
||||||
* @param heading the rotation of the Matrix
|
|
||||||
*/
|
|
||||||
public void setPrevRotationMatrix(double heading) {
|
|
||||||
prevRotationMatrix = new Matrix(3,3);
|
|
||||||
prevRotationMatrix.set(0, 0, Math.cos(heading));
|
|
||||||
prevRotationMatrix.set(0, 1, -Math.sin(heading));
|
|
||||||
prevRotationMatrix.set(1, 0, Math.sin(heading));
|
|
||||||
prevRotationMatrix.set(1, 1, Math.cos(heading));
|
|
||||||
prevRotationMatrix.set(2, 2, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the current pose estimate. Changing this should just change the robot's current
|
|
||||||
* pose estimate, not anything to do with the start pose.
|
|
||||||
*
|
|
||||||
* @param setPose the new current pose estimate
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setPose(Pose setPose) {
|
|
||||||
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
|
|
||||||
resetEncoders();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the elapsed time timer that keeps track of time between updates, as well as the
|
|
||||||
* change position of the Encoders. Then, the robot's global change in position is calculated
|
|
||||||
* using the pose exponential method.
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void update() {
|
|
||||||
deltaTimeNano = timer.getElapsedTime();
|
|
||||||
timer.resetTimer();
|
|
||||||
|
|
||||||
updateEncoders();
|
|
||||||
Matrix robotDeltas = getRobotDeltas();
|
|
||||||
Matrix globalDeltas;
|
|
||||||
setPrevRotationMatrix(getPose().getHeading());
|
|
||||||
|
|
||||||
Matrix transformation = new Matrix(3,3);
|
|
||||||
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
|
|
||||||
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
|
||||||
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
|
|
||||||
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
|
|
||||||
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
|
||||||
transformation.set(2, 2, 1.0);
|
|
||||||
} else {
|
|
||||||
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(2, 2, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
|
|
||||||
|
|
||||||
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
|
|
||||||
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
|
|
||||||
|
|
||||||
totalHeading += globalDeltas.get(2, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the Encoders.
|
|
||||||
*/
|
|
||||||
public void updateEncoders() {
|
|
||||||
leftFront.update();
|
|
||||||
rightFront.update();
|
|
||||||
leftRear.update();
|
|
||||||
rightRear.update();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This resets the Encoders.
|
|
||||||
*/
|
|
||||||
public void resetEncoders() {
|
|
||||||
leftFront.reset();
|
|
||||||
rightFront.reset();
|
|
||||||
leftRear.reset();
|
|
||||||
rightRear.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This calculates the change in position from the perspective of the robot using information
|
|
||||||
* from the Encoders.
|
|
||||||
*
|
|
||||||
* @return returns a Matrix containing the robot relative movement.
|
|
||||||
*/
|
|
||||||
public Matrix getRobotDeltas() {
|
|
||||||
Matrix returnMatrix = new Matrix(3,1);
|
|
||||||
// x/forward movement
|
|
||||||
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
|
|
||||||
//y/strafe movement
|
|
||||||
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
|
|
||||||
// theta/turning
|
|
||||||
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
|
|
||||||
return returnMatrix;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
|
|
||||||
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
|
|
||||||
*
|
|
||||||
* @return returns how far the robot has turned in total, in radians.
|
|
||||||
*/
|
|
||||||
public double getTotalHeading() {
|
|
||||||
return totalHeading;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to forward movement measurement to convert from encoder
|
|
||||||
* ticks to inches. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the forward ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getForwardMultiplier() {
|
|
||||||
return FORWARD_TICKS_TO_INCHES;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
|
|
||||||
* encoder ticks to inches. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the lateral/strafe ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getLateralMultiplier() {
|
|
||||||
return STRAFE_TICKS_TO_INCHES;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to turning movement measurement to convert from encoder
|
|
||||||
* ticks to radians. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the turning ticks to radians multiplier
|
|
||||||
*/
|
|
||||||
public double getTurningMultiplier() {
|
|
||||||
return TURN_TICKS_TO_RADIANS;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This does nothing since this localizer does not use the IMU.
|
|
||||||
*/
|
|
||||||
public void resetIMU() {
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,218 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
|
||||||
|
|
||||||
import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
|
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This is the OTOSLocalizer class. This class extends the Localizer superclass and is a
|
|
||||||
* localizer that uses the SparkFun OTOS. The diagram below, which is modified from
|
|
||||||
* Road Runner, shows a typical set up.
|
|
||||||
*
|
|
||||||
* The view is from the top of the robot looking downwards.
|
|
||||||
*
|
|
||||||
* left on robot is the y positive direction
|
|
||||||
*
|
|
||||||
* forward on robot is the x positive direction
|
|
||||||
*
|
|
||||||
* /--------------\
|
|
||||||
* | ____ |
|
|
||||||
* | ---- |
|
|
||||||
* | || || |
|
|
||||||
* | || || | ----> left (y positive)
|
|
||||||
* | |
|
|
||||||
* | |
|
|
||||||
* \--------------/
|
|
||||||
* |
|
|
||||||
* |
|
|
||||||
* V
|
|
||||||
* forward (x positive)
|
|
||||||
*
|
|
||||||
* @author Anyi Lin - 10158 Scott's Bots
|
|
||||||
* @version 1.0, 7/20/2024
|
|
||||||
*/
|
|
||||||
public class OTOSLocalizer extends Localizer {
|
|
||||||
private HardwareMap hardwareMap;
|
|
||||||
private Pose startPose;
|
|
||||||
private SparkFunOTOS otos;
|
|
||||||
private double previousHeading;
|
|
||||||
private double totalHeading;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
|
||||||
* facing 0 heading.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
*/
|
|
||||||
public OTOSLocalizer(HardwareMap map) {
|
|
||||||
this(map, new Pose());
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose
|
|
||||||
* specifying the starting pose of the localizer.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
* @param setStartPose the Pose to start from
|
|
||||||
*/
|
|
||||||
public OTOSLocalizer(HardwareMap map, Pose setStartPose) {
|
|
||||||
hardwareMap = map;
|
|
||||||
|
|
||||||
// TODO: replace this with your OTOS port
|
|
||||||
/*
|
|
||||||
TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the
|
|
||||||
'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a
|
|
||||||
"SparkFunOTOS Corrected" in your robot confg
|
|
||||||
*/
|
|
||||||
SparkFunOTOS
|
|
||||||
otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
|
|
||||||
|
|
||||||
otos.setLinearUnit(DistanceUnit.INCH);
|
|
||||||
otos.setAngularUnit(AngleUnit.RADIANS);
|
|
||||||
|
|
||||||
// TODO: replace this with your OTOS offset from the center of the robot
|
|
||||||
// For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being
|
|
||||||
// positive y and forward being positive x. PI/2 radians is facing forward, and clockwise
|
|
||||||
// rotation is negative rotation.
|
|
||||||
otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2));
|
|
||||||
|
|
||||||
// TODO: replace these with your tuned multipliers
|
|
||||||
otos.setLinearScalar(1.0);
|
|
||||||
otos.setAngularScalar(1.0);
|
|
||||||
|
|
||||||
otos.calibrateImu();
|
|
||||||
otos.resetTracking();
|
|
||||||
|
|
||||||
setStartPose(setStartPose);
|
|
||||||
totalHeading = 0;
|
|
||||||
previousHeading = startPose.getHeading();
|
|
||||||
|
|
||||||
resetOTOS();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current pose estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current pose estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getPose() {
|
|
||||||
SparkFunOTOS.Pose2D pose = otos.getPosition();
|
|
||||||
return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h));
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getVelocity() {
|
|
||||||
SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity();
|
|
||||||
return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Vector
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Vector getVelocityVector() {
|
|
||||||
return getVelocity().getVector();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the start pose. Changing the start pose should move the robot as if all its
|
|
||||||
* previous movements were displacing it from its new start pose.
|
|
||||||
*
|
|
||||||
* @param setStart the new start pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setStartPose(Pose setStart) {
|
|
||||||
startPose = setStart;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the current pose estimate. Changing this should just change the robot's current
|
|
||||||
* pose estimate, not anything to do with the start pose.
|
|
||||||
*
|
|
||||||
* @param setPose the new current pose estimate
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setPose(Pose setPose) {
|
|
||||||
resetOTOS();
|
|
||||||
Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose);
|
|
||||||
otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading()));
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the total heading of the robot. The OTOS handles all other updates itself.
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void update() {
|
|
||||||
totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading);
|
|
||||||
previousHeading = otos.getPosition().h;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This resets the OTOS.
|
|
||||||
*/
|
|
||||||
public void resetOTOS() {
|
|
||||||
otos.resetTracking();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
|
|
||||||
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
|
|
||||||
*
|
|
||||||
* @return returns how far the robot has turned in total, in radians.
|
|
||||||
*/
|
|
||||||
public double getTotalHeading() {
|
|
||||||
return totalHeading;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to forward movement measurement to convert from OTOS
|
|
||||||
* ticks to inches. For the OTOS, this value is the same as the lateral multiplier.
|
|
||||||
* This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the forward ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getForwardMultiplier() {
|
|
||||||
return otos.getLinearScalar();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
|
|
||||||
* OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier.
|
|
||||||
* This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the lateral/strafe ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getLateralMultiplier() {
|
|
||||||
return otos.getLinearScalar();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to turning movement measurement to convert from OTOS ticks
|
|
||||||
* to radians. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the turning ticks to radians multiplier
|
|
||||||
*/
|
|
||||||
public double getTurningMultiplier() {
|
|
||||||
return otos.getAngularScalar();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This does nothing since this localizer does not use the IMU.
|
|
||||||
*/
|
|
||||||
public void resetIMU() {
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,159 +0,0 @@
|
|||||||
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
|
||||||
//
|
|
||||||
//import com.acmerobotics.roadrunner.geometry.Pose2d;
|
|
||||||
//import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
//
|
|
||||||
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
|
||||||
//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
|
||||||
//
|
|
||||||
//import java.util.ArrayList;
|
|
||||||
//import java.util.List;
|
|
||||||
//
|
|
||||||
///**
|
|
||||||
// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and
|
|
||||||
// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing
|
|
||||||
// * localizer system.
|
|
||||||
// *
|
|
||||||
// * @author Anyi Lin - 10158 Scott's Bots
|
|
||||||
// * @version 1.0, 5/9/2024
|
|
||||||
// */
|
|
||||||
//public class RRToPedroThreeWheelLocalizer extends Localizer {
|
|
||||||
// private RoadRunnerThreeWheelLocalizer localizer;
|
|
||||||
// private double totalHeading;
|
|
||||||
// private Pose startPose;
|
|
||||||
// private Pose previousPose;
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously
|
|
||||||
// * used Road Runner localization system to the new Pedro Pathing localization system.
|
|
||||||
// *
|
|
||||||
// * @param hardwareMap the HardwareMap
|
|
||||||
// */
|
|
||||||
// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) {
|
|
||||||
// List<Integer> lastTrackingEncPositions = new ArrayList<>();
|
|
||||||
// List<Integer> lastTrackingEncVels = new ArrayList<>();
|
|
||||||
//
|
|
||||||
// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
|
|
||||||
//
|
|
||||||
// startPose = new Pose();
|
|
||||||
// previousPose = new Pose();
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns the current pose estimate as a Pose.
|
|
||||||
// *
|
|
||||||
// * @return returns the current pose estimate
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public Pose getPose() {
|
|
||||||
// Pose2d pose = localizer.getPoseEstimate();
|
|
||||||
// return new Pose(pose.getX(), pose.getY(), pose.getHeading());
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns the current velocity estimate as a Pose.
|
|
||||||
// *
|
|
||||||
// * @return returns the current velocity estimate
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public Pose getVelocity() {
|
|
||||||
// Pose2d pose = localizer.getPoseVelocity();
|
|
||||||
// return new Pose(pose.getX(), pose.getY(), pose.getHeading());
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns the current velocity estimate as a Vector.
|
|
||||||
// *
|
|
||||||
// * @return returns the current velocity estimate
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public Vector getVelocityVector() {
|
|
||||||
// Pose2d pose = localizer.getPoseVelocity();
|
|
||||||
// Vector returnVector = new Vector();
|
|
||||||
// returnVector.setOrthogonalComponents(pose.getX(), pose.getY());
|
|
||||||
// return returnVector;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This sets the start pose. Any movement of the robot is treated as a displacement from the
|
|
||||||
// * start pose, so moving the start pose will move the current pose estimate the same amount.
|
|
||||||
// *
|
|
||||||
// * @param setStart the new start pose
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public void setStartPose(Pose setStart) {
|
|
||||||
// Pose oldStart = startPose;
|
|
||||||
// startPose = setStart;
|
|
||||||
// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart);
|
|
||||||
// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading()));
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This sets the current pose estimate. This has no effect on the start pose.
|
|
||||||
// *
|
|
||||||
// * @param setPose the new current pose estimate
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public void setPose(Pose setPose) {
|
|
||||||
// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading()));
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This updates the total heading and previous pose estimate. Everything else is handled by the
|
|
||||||
// * Road Runner localizer on its own, but updating this tells you how far the robot has really
|
|
||||||
// * turned.
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public void update() {
|
|
||||||
// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading());
|
|
||||||
// previousPose = getPose();
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns how far the robot has actually turned.
|
|
||||||
// *
|
|
||||||
// * @return returns the total angle turned, in degrees.
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public double getTotalHeading() {
|
|
||||||
// return totalHeading;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns the forward multiplier of the Road Runner localizer, which converts from ticks
|
|
||||||
// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
|
|
||||||
// * multiplied together should be. If you do use that, then do be aware that the value returned is
|
|
||||||
// * the product of the Road Runner ticks to inches and the x multiplier.
|
|
||||||
// *
|
|
||||||
// * @return returns the forward multiplier
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public double getForwardMultiplier() {
|
|
||||||
// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks
|
|
||||||
// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
|
|
||||||
// * multiplied together should be. If you do use that, then do be aware that the value returned is
|
|
||||||
// * the product of the Road Runner ticks to inches and the y multiplier.
|
|
||||||
// *
|
|
||||||
// * @return returns the lateral multiplier
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public double getLateralMultiplier() {
|
|
||||||
// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist.
|
|
||||||
// * There really isn't a point in tuning the turning for the Road Runner localizer. This will
|
|
||||||
// * actually just return the average of the two other multipliers.
|
|
||||||
// *
|
|
||||||
// * @return returns the turning multiplier
|
|
||||||
// */
|
|
||||||
// @Override
|
|
||||||
// public double getTurningMultiplier() {
|
|
||||||
// return (getForwardMultiplier() + getLateralMultiplier()) / 2;
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
@ -1,132 +0,0 @@
|
|||||||
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
|
||||||
//
|
|
||||||
//import com.acmerobotics.roadrunner.util.NanoClock;
|
|
||||||
//import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
//import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
|
||||||
//
|
|
||||||
///**
|
|
||||||
// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a
|
|
||||||
// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected
|
|
||||||
// * velocity counts and allow reversing independently of the corresponding slot's motor direction.
|
|
||||||
// *
|
|
||||||
// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have
|
|
||||||
// * import statements, so I'm not crediting myself as an author for this.
|
|
||||||
// *
|
|
||||||
// * @author Road Runner dev team
|
|
||||||
// * @version 1.0, 5/9/2024
|
|
||||||
// */
|
|
||||||
//public class RoadRunnerEncoder {
|
|
||||||
// private final static int CPS_STEP = 0x10000;
|
|
||||||
//
|
|
||||||
// private static double inverseOverflow(double input, double estimate) {
|
|
||||||
// // convert to uint16
|
|
||||||
// int real = (int) input & 0xffff;
|
|
||||||
// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
|
|
||||||
// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
|
|
||||||
// real += ((real % 20) / 4) * CPS_STEP;
|
|
||||||
// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
|
|
||||||
// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
|
|
||||||
// return real;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// public enum Direction {
|
|
||||||
// FORWARD(1),
|
|
||||||
// REVERSE(-1);
|
|
||||||
//
|
|
||||||
// private int multiplier;
|
|
||||||
//
|
|
||||||
// Direction(int multiplier) {
|
|
||||||
// this.multiplier = multiplier;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// public int getMultiplier() {
|
|
||||||
// return multiplier;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// private DcMotorEx motor;
|
|
||||||
// private NanoClock clock;
|
|
||||||
//
|
|
||||||
// private Direction direction;
|
|
||||||
//
|
|
||||||
// private int lastPosition;
|
|
||||||
// private int velocityEstimateIdx;
|
|
||||||
// private double[] velocityEstimates;
|
|
||||||
// private double lastUpdateTime;
|
|
||||||
//
|
|
||||||
// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
|
|
||||||
// this.motor = motor;
|
|
||||||
// this.clock = clock;
|
|
||||||
//
|
|
||||||
// this.direction = Direction.FORWARD;
|
|
||||||
//
|
|
||||||
// this.lastPosition = 0;
|
|
||||||
// this.velocityEstimates = new double[3];
|
|
||||||
// this.lastUpdateTime = clock.seconds();
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// public RoadRunnerEncoder(DcMotorEx motor) {
|
|
||||||
// this(motor, NanoClock.system());
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// public Direction getDirection() {
|
|
||||||
// return direction;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// private int getMultiplier() {
|
|
||||||
// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
|
|
||||||
// * @param direction either reverse or forward depending on if encoder counts should be negated
|
|
||||||
// */
|
|
||||||
// public void setDirection(Direction direction) {
|
|
||||||
// this.direction = direction;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * Gets the position from the underlying motor and adjusts for the set direction.
|
|
||||||
// * Additionally, this method updates the velocity estimates used for compensated velocity
|
|
||||||
// *
|
|
||||||
// * @return encoder position
|
|
||||||
// */
|
|
||||||
// public int getCurrentPosition() {
|
|
||||||
// int multiplier = getMultiplier();
|
|
||||||
// int currentPosition = motor.getCurrentPosition() * multiplier;
|
|
||||||
// if (currentPosition != lastPosition) {
|
|
||||||
// double currentTime = clock.seconds();
|
|
||||||
// double dt = currentTime - lastUpdateTime;
|
|
||||||
// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
|
|
||||||
// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
|
|
||||||
// lastPosition = currentPosition;
|
|
||||||
// lastUpdateTime = currentTime;
|
|
||||||
// }
|
|
||||||
// return currentPosition;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * Gets the velocity directly from the underlying motor and compensates for the direction
|
|
||||||
// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
|
|
||||||
// *
|
|
||||||
// * @return raw velocity
|
|
||||||
// */
|
|
||||||
// public double getRawVelocity() {
|
|
||||||
// int multiplier = getMultiplier();
|
|
||||||
// return motor.getVelocity() * multiplier;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// /**
|
|
||||||
// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
|
|
||||||
// * that are lost in overflow due to velocity being transmitted as 16 bits.
|
|
||||||
// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
|
|
||||||
// *
|
|
||||||
// * @return corrected velocity
|
|
||||||
// */
|
|
||||||
// public double getCorrectedVelocity() {
|
|
||||||
// double median = velocityEstimates[0] > velocityEstimates[1]
|
|
||||||
// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
|
|
||||||
// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
|
|
||||||
// return inverseOverflow(getRawVelocity(), median);
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
@ -1,123 +0,0 @@
|
|||||||
//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
|
|
||||||
//
|
|
||||||
//import androidx.annotation.NonNull;
|
|
||||||
//
|
|
||||||
//import com.acmerobotics.dashboard.config.Config;
|
|
||||||
//import com.acmerobotics.roadrunner.geometry.Pose2d;
|
|
||||||
//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
|
|
||||||
//import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
//import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
//
|
|
||||||
//import java.util.Arrays;
|
|
||||||
//import java.util.List;
|
|
||||||
//
|
|
||||||
///*
|
|
||||||
// * Sample tracking wheel localizer implementation assuming the standard configuration:
|
|
||||||
// *
|
|
||||||
// * left on robot is y pos
|
|
||||||
// *
|
|
||||||
// * front of robot is x pos
|
|
||||||
// *
|
|
||||||
// * /--------------\
|
|
||||||
// * | ____ |
|
|
||||||
// * | ---- |
|
|
||||||
// * | || || |
|
|
||||||
// * | || || |
|
|
||||||
// * | |
|
|
||||||
// * | |
|
|
||||||
// * \--------------/
|
|
||||||
// *
|
|
||||||
// */
|
|
||||||
//
|
|
||||||
///**
|
|
||||||
// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
|
|
||||||
// * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an
|
|
||||||
// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to
|
|
||||||
// * Pedro Pathing to avoid having imports.
|
|
||||||
// *
|
|
||||||
// * @author Road Runner dev team
|
|
||||||
// * @author Anyi Lin - 10158 Scott's Bots
|
|
||||||
// * @version 1.0, 5/9/2024
|
|
||||||
// */
|
|
||||||
//@Config
|
|
||||||
//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
|
|
||||||
// public static double TICKS_PER_REV = 8192;
|
|
||||||
// public static double WHEEL_RADIUS = 1.37795; // in
|
|
||||||
// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
|
|
||||||
//
|
|
||||||
// public static double X_MULTIPLIER = 0.5008239963;
|
|
||||||
// public static double Y_MULTIPLIER = 0.5018874659;
|
|
||||||
//
|
|
||||||
// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23;
|
|
||||||
//
|
|
||||||
// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
|
|
||||||
//
|
|
||||||
// private List<Integer> lastEncPositions, lastEncVels;
|
|
||||||
//
|
|
||||||
// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
|
|
||||||
// super(Arrays.asList(
|
|
||||||
// new Pose2d(leftX, leftY, 0), // left
|
|
||||||
// new Pose2d(rightX, rightY, 0), // right
|
|
||||||
// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe
|
|
||||||
// ));
|
|
||||||
//
|
|
||||||
// lastEncPositions = lastTrackingEncPositions;
|
|
||||||
// lastEncVels = lastTrackingEncVels;
|
|
||||||
//
|
|
||||||
// // TODO: redo the configs here
|
|
||||||
// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
|
|
||||||
// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
|
|
||||||
// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
|
|
||||||
//
|
|
||||||
// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
|
|
||||||
// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
|
|
||||||
// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
|
|
||||||
// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// public void resetHeading(double heading) {
|
|
||||||
// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// public static double encoderTicksToInches(double ticks) {
|
|
||||||
// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// @NonNull
|
|
||||||
// @Override
|
|
||||||
// public List<Double> getWheelPositions() {
|
|
||||||
// int leftPos = leftEncoder.getCurrentPosition();
|
|
||||||
// int rightPos = rightEncoder.getCurrentPosition();
|
|
||||||
// int frontPos = strafeEncoder.getCurrentPosition();
|
|
||||||
//
|
|
||||||
// lastEncPositions.clear();
|
|
||||||
// lastEncPositions.add(leftPos);
|
|
||||||
// lastEncPositions.add(rightPos);
|
|
||||||
// lastEncPositions.add(frontPos);
|
|
||||||
//
|
|
||||||
// return Arrays.asList(
|
|
||||||
// encoderTicksToInches(leftPos) * X_MULTIPLIER,
|
|
||||||
// encoderTicksToInches(rightPos) * X_MULTIPLIER,
|
|
||||||
// encoderTicksToInches(frontPos) * Y_MULTIPLIER
|
|
||||||
// );
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// @NonNull
|
|
||||||
// @Override
|
|
||||||
// public List<Double> getWheelVelocities() {
|
|
||||||
// int leftVel = (int) leftEncoder.getCorrectedVelocity();
|
|
||||||
// int rightVel = (int) rightEncoder.getCorrectedVelocity();
|
|
||||||
// int frontVel = (int) strafeEncoder.getCorrectedVelocity();
|
|
||||||
//
|
|
||||||
// lastEncVels.clear();
|
|
||||||
// lastEncVels.add(leftVel);
|
|
||||||
// lastEncVels.add(rightVel);
|
|
||||||
// lastEncVels.add(frontVel);
|
|
||||||
//
|
|
||||||
// return Arrays.asList(
|
|
||||||
// encoderTicksToInches(leftVel) * X_MULTIPLIER,
|
|
||||||
// encoderTicksToInches(rightVel) * X_MULTIPLIER,
|
|
||||||
// encoderTicksToInches(frontVel) * Y_MULTIPLIER
|
|
||||||
// );
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
@ -1,317 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.*;
|
|
||||||
|
|
||||||
import com.acmerobotics.dashboard.config.Config;
|
|
||||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
import com.qualcomm.robotcore.hardware.IMU;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a
|
|
||||||
* localizer that uses the three wheel odometry set up with the IMU to have more accurate heading
|
|
||||||
* readings. The diagram below, which is modified from Road Runner, shows a typical set up.
|
|
||||||
*
|
|
||||||
* The view is from the top of the robot looking downwards.
|
|
||||||
*
|
|
||||||
* left on robot is the y positive direction
|
|
||||||
*
|
|
||||||
* forward on robot is the x positive direction
|
|
||||||
*
|
|
||||||
* /--------------\
|
|
||||||
* | ____ |
|
|
||||||
* | ---- |
|
|
||||||
* | || || |
|
|
||||||
* | || || | ----> left (y positive)
|
|
||||||
* | |
|
|
||||||
* | |
|
|
||||||
* \--------------/
|
|
||||||
* |
|
|
||||||
* |
|
|
||||||
* V
|
|
||||||
* forward (x positive)
|
|
||||||
*
|
|
||||||
* @author Logan Nash
|
|
||||||
* @author Anyi Lin - 10158 Scott's Bots
|
|
||||||
* @version 1.0, 7/9/2024
|
|
||||||
*/
|
|
||||||
@Config
|
|
||||||
public class ThreeWheelIMULocalizer extends Localizer {
|
|
||||||
private HardwareMap hardwareMap;
|
|
||||||
private Pose startPose;
|
|
||||||
private Pose displacementPose;
|
|
||||||
private Pose currentVelocity;
|
|
||||||
private Matrix prevRotationMatrix;
|
|
||||||
private NanoTimer timer;
|
|
||||||
private long deltaTimeNano;
|
|
||||||
private Encoder leftEncoder;
|
|
||||||
private Encoder rightEncoder;
|
|
||||||
private Encoder strafeEncoder;
|
|
||||||
private Pose leftEncoderPose;
|
|
||||||
private Pose rightEncoderPose;
|
|
||||||
private Pose strafeEncoderPose;
|
|
||||||
|
|
||||||
public final IMU imu;
|
|
||||||
private double previousIMUOrientation;
|
|
||||||
private double deltaRadians;
|
|
||||||
private double totalHeading;
|
|
||||||
public static double FORWARD_TICKS_TO_INCHES = 0.004;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
|
||||||
public static double STRAFE_TICKS_TO_INCHES = -0.0036;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
|
||||||
public static double TURN_TICKS_TO_RADIANS = 0.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
|
||||||
|
|
||||||
public static boolean useIMU = true;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0)
|
|
||||||
* facing 0 heading.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
*/
|
|
||||||
public ThreeWheelIMULocalizer(HardwareMap map) {
|
|
||||||
this(map, new Pose());
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose
|
|
||||||
* specifying the starting pose of the localizer.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
* @param setStartPose the Pose to start from
|
|
||||||
*/
|
|
||||||
public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) {
|
|
||||||
hardwareMap = map;
|
|
||||||
imu = hardwareMap.get(IMU.class, IMU);
|
|
||||||
|
|
||||||
// TODO: replace this with your IMU's orientation
|
|
||||||
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
|
|
||||||
|
|
||||||
// TODO: replace these with your encoder positions
|
|
||||||
leftEncoderPose = new Pose(-7.625, 6.19375, 0);
|
|
||||||
rightEncoderPose = new Pose(-7.625, -6.19375, 0);
|
|
||||||
strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
|
|
||||||
|
|
||||||
// TODO: replace these with your encoder ports
|
|
||||||
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
|
||||||
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
|
|
||||||
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
|
|
||||||
|
|
||||||
// TODO: reverse any encoders necessary
|
|
||||||
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
|
|
||||||
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
|
|
||||||
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
|
|
||||||
|
|
||||||
setStartPose(setStartPose);
|
|
||||||
timer = new NanoTimer();
|
|
||||||
deltaTimeNano = 1;
|
|
||||||
displacementPose = new Pose();
|
|
||||||
currentVelocity = new Pose();
|
|
||||||
totalHeading = 0;
|
|
||||||
|
|
||||||
resetEncoders();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current pose estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current pose estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getPose() {
|
|
||||||
return MathFunctions.addPoses(startPose, displacementPose);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getVelocity() {
|
|
||||||
return currentVelocity.copy();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Vector
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Vector getVelocityVector() {
|
|
||||||
return currentVelocity.getVector();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the start pose. Changing the start pose should move the robot as if all its
|
|
||||||
* previous movements were displacing it from its new start pose.
|
|
||||||
*
|
|
||||||
* @param setStart the new start pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setStartPose(Pose setStart) {
|
|
||||||
startPose = setStart;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the Matrix that contains the previous pose's heading rotation.
|
|
||||||
*
|
|
||||||
* @param heading the rotation of the Matrix
|
|
||||||
*/
|
|
||||||
public void setPrevRotationMatrix(double heading) {
|
|
||||||
prevRotationMatrix = new Matrix(3,3);
|
|
||||||
prevRotationMatrix.set(0, 0, Math.cos(heading));
|
|
||||||
prevRotationMatrix.set(0, 1, -Math.sin(heading));
|
|
||||||
prevRotationMatrix.set(1, 0, Math.sin(heading));
|
|
||||||
prevRotationMatrix.set(1, 1, Math.cos(heading));
|
|
||||||
prevRotationMatrix.set(2, 2, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the current pose estimate. Changing this should just change the robot's current
|
|
||||||
* pose estimate, not anything to do with the start pose.
|
|
||||||
*
|
|
||||||
* @param setPose the new current pose estimate
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setPose(Pose setPose) {
|
|
||||||
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
|
|
||||||
resetEncoders();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the elapsed time timer that keeps track of time between updates, as well as the
|
|
||||||
* change position of the Encoders. Then, the robot's global change in position is calculated
|
|
||||||
* using the pose exponential method.
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void update() {
|
|
||||||
deltaTimeNano = timer.getElapsedTime();
|
|
||||||
timer.resetTimer();
|
|
||||||
|
|
||||||
updateEncoders();
|
|
||||||
Matrix robotDeltas = getRobotDeltas();
|
|
||||||
Matrix globalDeltas;
|
|
||||||
setPrevRotationMatrix(getPose().getHeading());
|
|
||||||
|
|
||||||
Matrix transformation = new Matrix(3,3);
|
|
||||||
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
|
|
||||||
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
|
||||||
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
|
|
||||||
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
|
|
||||||
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
|
||||||
transformation.set(2, 2, 1.0);
|
|
||||||
} else {
|
|
||||||
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(2, 2, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
|
|
||||||
|
|
||||||
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
|
|
||||||
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
|
|
||||||
|
|
||||||
totalHeading += globalDeltas.get(2, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the Encoders.
|
|
||||||
*/
|
|
||||||
public void updateEncoders() {
|
|
||||||
leftEncoder.update();
|
|
||||||
rightEncoder.update();
|
|
||||||
strafeEncoder.update();
|
|
||||||
|
|
||||||
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
|
|
||||||
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
|
|
||||||
previousIMUOrientation = currentIMUOrientation;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This resets the Encoders.
|
|
||||||
*/
|
|
||||||
public void resetEncoders() {
|
|
||||||
leftEncoder.reset();
|
|
||||||
rightEncoder.reset();
|
|
||||||
strafeEncoder.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This calculates the change in position from the perspective of the robot using information
|
|
||||||
* from the Encoders.
|
|
||||||
*
|
|
||||||
* @return returns a Matrix containing the robot relative movement.
|
|
||||||
*/
|
|
||||||
public Matrix getRobotDeltas() {
|
|
||||||
Matrix returnMatrix = new Matrix(3,1);
|
|
||||||
// x/forward movement
|
|
||||||
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
|
|
||||||
//y/strafe movement
|
|
||||||
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))));
|
|
||||||
// theta/turning
|
|
||||||
if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) {
|
|
||||||
returnMatrix.set(2, 0, deltaRadians);
|
|
||||||
} else {
|
|
||||||
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
|
|
||||||
}
|
|
||||||
return returnMatrix;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
|
|
||||||
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
|
|
||||||
*
|
|
||||||
* @return returns how far the robot has turned in total, in radians.
|
|
||||||
*/
|
|
||||||
public double getTotalHeading() {
|
|
||||||
return totalHeading;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to forward movement measurement to convert from encoder
|
|
||||||
* ticks to inches. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the forward ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getForwardMultiplier() {
|
|
||||||
return FORWARD_TICKS_TO_INCHES;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
|
|
||||||
* encoder ticks to inches. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the lateral/strafe ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getLateralMultiplier() {
|
|
||||||
return STRAFE_TICKS_TO_INCHES;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to turning movement measurement to convert from encoder
|
|
||||||
* ticks to radians. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the turning ticks to radians multiplier
|
|
||||||
*/
|
|
||||||
public double getTurningMultiplier() {
|
|
||||||
return TURN_TICKS_TO_RADIANS;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This resets the IMU.
|
|
||||||
*/
|
|
||||||
public void resetIMU() {
|
|
||||||
imu.resetYaw();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -57,12 +57,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
|||||||
private Pose rightEncoderPose;
|
private Pose rightEncoderPose;
|
||||||
private Pose strafeEncoderPose;
|
private Pose strafeEncoderPose;
|
||||||
private double totalHeading;
|
private double totalHeading;
|
||||||
// public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
public static double FORWARD_TICKS_TO_INCHES = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
||||||
public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
public static double STRAFE_TICKS_TO_INCHES = -0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||||
// public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
public static double TURN_TICKS_TO_RADIANS = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
||||||
public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
|
||||||
// public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
|
||||||
public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
* This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
||||||
@ -83,9 +80,9 @@ public class ThreeWheelLocalizer extends Localizer {
|
|||||||
*/
|
*/
|
||||||
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
|
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
|
||||||
// TODO: replace these with your encoder positions
|
// TODO: replace these with your encoder positions
|
||||||
leftEncoderPose = new Pose(0, 6.19375, 0);
|
leftEncoderPose = new Pose(0.25, 6.25, 0);
|
||||||
rightEncoderPose = new Pose(0, -6.19375, 0);
|
rightEncoderPose = new Pose(0.25, -6.25, 0);
|
||||||
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
|
strafeEncoderPose = new Pose(-7, 0.25, Math.toRadians(90));
|
||||||
|
|
||||||
hardwareMap = map;
|
hardwareMap = map;
|
||||||
|
|
||||||
|
|||||||
@ -1,302 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
|
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
|
||||||
|
|
||||||
import com.acmerobotics.dashboard.config.Config;
|
|
||||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
|
||||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
||||||
import com.qualcomm.robotcore.hardware.IMU;
|
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a
|
|
||||||
* localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from
|
|
||||||
* Road Runner, shows a typical set up.
|
|
||||||
*
|
|
||||||
* The view is from the top of the robot looking downwards.
|
|
||||||
*
|
|
||||||
* left on robot is the y positive direction
|
|
||||||
*
|
|
||||||
* forward on robot is the x positive direction
|
|
||||||
*
|
|
||||||
* /--------------\
|
|
||||||
* | ____ |
|
|
||||||
* | ---- |
|
|
||||||
* | || || |
|
|
||||||
* | || || | ----> left (y positive)
|
|
||||||
* | |
|
|
||||||
* | |
|
|
||||||
* \--------------/
|
|
||||||
* |
|
|
||||||
* |
|
|
||||||
* V
|
|
||||||
* forward (x positive)
|
|
||||||
*
|
|
||||||
* @author Anyi Lin - 10158 Scott's Bots
|
|
||||||
* @version 1.0, 4/2/2024
|
|
||||||
*/
|
|
||||||
@Config
|
|
||||||
public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work
|
|
||||||
private HardwareMap hardwareMap;
|
|
||||||
private IMU imu;
|
|
||||||
private Pose startPose;
|
|
||||||
private Pose displacementPose;
|
|
||||||
private Pose currentVelocity;
|
|
||||||
private Matrix prevRotationMatrix;
|
|
||||||
private NanoTimer timer;
|
|
||||||
private long deltaTimeNano;
|
|
||||||
private Encoder forwardEncoder;
|
|
||||||
private Encoder strafeEncoder;
|
|
||||||
private Pose forwardEncoderPose;
|
|
||||||
private Pose strafeEncoderPose;
|
|
||||||
private double previousIMUOrientation;
|
|
||||||
private double deltaRadians;
|
|
||||||
private double totalHeading;
|
|
||||||
public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
|
|
||||||
public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
|
|
||||||
* facing 0 heading.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
*/
|
|
||||||
public TwoWheelLocalizer(HardwareMap map) {
|
|
||||||
this(map, new Pose());
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose
|
|
||||||
* specifying the starting pose of the localizer.
|
|
||||||
*
|
|
||||||
* @param map the HardwareMap
|
|
||||||
* @param setStartPose the Pose to start from
|
|
||||||
*/
|
|
||||||
public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) {
|
|
||||||
// TODO: replace these with your encoder positions
|
|
||||||
forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0);
|
|
||||||
strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90));
|
|
||||||
|
|
||||||
hardwareMap = map;
|
|
||||||
|
|
||||||
imu = hardwareMap.get(IMU.class, "imu");
|
|
||||||
// TODO: replace this with your IMU's orientation
|
|
||||||
imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT)));
|
|
||||||
|
|
||||||
// TODO: replace these with your encoder ports
|
|
||||||
forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
|
||||||
strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
|
|
||||||
|
|
||||||
// TODO: reverse any encoders necessary
|
|
||||||
forwardEncoder.setDirection(Encoder.REVERSE);
|
|
||||||
strafeEncoder.setDirection(Encoder.FORWARD);
|
|
||||||
|
|
||||||
setStartPose(setStartPose);
|
|
||||||
timer = new NanoTimer();
|
|
||||||
deltaTimeNano = 1;
|
|
||||||
displacementPose = new Pose();
|
|
||||||
currentVelocity = new Pose();
|
|
||||||
|
|
||||||
previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
|
|
||||||
deltaRadians = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current pose estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current pose estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getPose() {
|
|
||||||
return MathFunctions.addPoses(startPose, displacementPose);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Pose getVelocity() {
|
|
||||||
return currentVelocity.copy();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the current velocity estimate.
|
|
||||||
*
|
|
||||||
* @return returns the current velocity estimate as a Vector
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public Vector getVelocityVector() {
|
|
||||||
return currentVelocity.getVector();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the start pose. Changing the start pose should move the robot as if all its
|
|
||||||
* previous movements were displacing it from its new start pose.
|
|
||||||
*
|
|
||||||
* @param setStart the new start pose
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setStartPose(Pose setStart) {
|
|
||||||
startPose = setStart;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the Matrix that contains the previous pose's heading rotation.
|
|
||||||
*
|
|
||||||
* @param heading the rotation of the Matrix
|
|
||||||
*/
|
|
||||||
public void setPrevRotationMatrix(double heading) {
|
|
||||||
prevRotationMatrix = new Matrix(3,3);
|
|
||||||
prevRotationMatrix.set(0, 0, Math.cos(heading));
|
|
||||||
prevRotationMatrix.set(0, 1, -Math.sin(heading));
|
|
||||||
prevRotationMatrix.set(1, 0, Math.sin(heading));
|
|
||||||
prevRotationMatrix.set(1, 1, Math.cos(heading));
|
|
||||||
prevRotationMatrix.set(2, 2, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This sets the current pose estimate. Changing this should just change the robot's current
|
|
||||||
* pose estimate, not anything to do with the start pose.
|
|
||||||
*
|
|
||||||
* @param setPose the new current pose estimate
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void setPose(Pose setPose) {
|
|
||||||
displacementPose = MathFunctions.subtractPoses(setPose, startPose);
|
|
||||||
resetEncoders();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the elapsed time timer that keeps track of time between updates, as well as the
|
|
||||||
* change position of the Encoders and the IMU readings. Then, the robot's global change in
|
|
||||||
* position is calculated using the pose exponential method.
|
|
||||||
*/
|
|
||||||
@Override
|
|
||||||
public void update() {
|
|
||||||
deltaTimeNano = timer.getElapsedTime();
|
|
||||||
timer.resetTimer();
|
|
||||||
|
|
||||||
updateEncoders();
|
|
||||||
Matrix robotDeltas = getRobotDeltas();
|
|
||||||
Matrix globalDeltas;
|
|
||||||
setPrevRotationMatrix(getPose().getHeading());
|
|
||||||
|
|
||||||
Matrix transformation = new Matrix(3,3);
|
|
||||||
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
|
|
||||||
transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
|
||||||
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
|
|
||||||
transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
|
|
||||||
transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
|
|
||||||
transformation.set(2, 2, 1.0);
|
|
||||||
} else {
|
|
||||||
transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
|
|
||||||
transformation.set(2, 2, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
|
|
||||||
|
|
||||||
displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
|
|
||||||
currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9)));
|
|
||||||
|
|
||||||
totalHeading += globalDeltas.get(2, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This updates the Encoders as well as the IMU.
|
|
||||||
*/
|
|
||||||
public void updateEncoders() {
|
|
||||||
forwardEncoder.update();
|
|
||||||
strafeEncoder.update();
|
|
||||||
|
|
||||||
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
|
|
||||||
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
|
|
||||||
previousIMUOrientation = currentIMUOrientation;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This resets the Encoders.
|
|
||||||
*/
|
|
||||||
public void resetEncoders() {
|
|
||||||
forwardEncoder.reset();
|
|
||||||
strafeEncoder.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This calculates the change in position from the perspective of the robot using information
|
|
||||||
* from the Encoders and IMU.
|
|
||||||
*
|
|
||||||
* @return returns a Matrix containing the robot relative movement.
|
|
||||||
*/
|
|
||||||
public Matrix getRobotDeltas() {
|
|
||||||
Matrix returnMatrix = new Matrix(3,1);
|
|
||||||
// x/forward movement
|
|
||||||
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
|
|
||||||
//y/strafe movement
|
|
||||||
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
|
|
||||||
// theta/turning
|
|
||||||
returnMatrix.set(2,0, deltaRadians);
|
|
||||||
return returnMatrix;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns how far the robot has turned in radians, in a number not clamped between 0 and
|
|
||||||
* 2 * pi radians. This is used for some tuning things and nothing actually within the following.
|
|
||||||
*
|
|
||||||
* @return returns how far the robot has turned in total, in radians.
|
|
||||||
*/
|
|
||||||
public double getTotalHeading() {
|
|
||||||
return totalHeading;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to forward movement measurement to convert from encoder
|
|
||||||
* ticks to inches. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the forward ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getForwardMultiplier() {
|
|
||||||
return FORWARD_TICKS_TO_INCHES;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to lateral/strafe movement measurement to convert from
|
|
||||||
* encoder ticks to inches. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the lateral/strafe ticks to inches multiplier
|
|
||||||
*/
|
|
||||||
public double getLateralMultiplier() {
|
|
||||||
return STRAFE_TICKS_TO_INCHES;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This returns the multiplier applied to turning movement measurement to convert from encoder
|
|
||||||
* ticks to radians. This is found empirically through a tuner.
|
|
||||||
*
|
|
||||||
* @return returns the turning ticks to radians multiplier
|
|
||||||
*/
|
|
||||||
public double getTurningMultiplier() {
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This resets the IMU.
|
|
||||||
*/
|
|
||||||
public void resetIMU() {
|
|
||||||
imu.resetYaw();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -63,7 +63,6 @@ public class LateralTuner extends OpMode {
|
|||||||
telemetryA.addData("distance moved", poseUpdater.getPose().getY());
|
telemetryA.addData("distance moved", poseUpdater.getPose().getY());
|
||||||
telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
|
telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
|
||||||
telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
|
telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
|
||||||
|
|
||||||
telemetryA.update();
|
telemetryA.update();
|
||||||
|
|
||||||
Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
|
Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");
|
||||||
|
|||||||
@ -1,5 +1,9 @@
|
|||||||
package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
|
package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
|
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.leftRearMotorName;
|
||||||
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
|
import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
|
||||||
@ -58,8 +62,10 @@ public class LocalizationTest extends OpMode {
|
|||||||
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
|
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
|
||||||
rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
|
rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
|
||||||
|
|
||||||
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
|
||||||
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
|
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
|
||||||
|
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
||||||
|
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
||||||
|
|
||||||
motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
|
motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
|
||||||
|
|
||||||
|
|||||||
@ -40,9 +40,9 @@ public class FollowerConstants {
|
|||||||
|
|
||||||
// Translational PIDF coefficients (don't use integral)
|
// Translational PIDF coefficients (don't use integral)
|
||||||
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
|
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
|
||||||
0.1,
|
.25,
|
||||||
0,
|
0,
|
||||||
0.01,
|
0.0375,
|
||||||
0);
|
0);
|
||||||
|
|
||||||
// Translational Integral
|
// Translational Integral
|
||||||
@ -53,14 +53,14 @@ public class FollowerConstants {
|
|||||||
0);
|
0);
|
||||||
|
|
||||||
// Feed forward constant added on to the translational PIDF
|
// Feed forward constant added on to the translational PIDF
|
||||||
public static double translationalPIDFFeedForward = 0.015;
|
public static double translationalPIDFFeedForward = 0.00;
|
||||||
|
|
||||||
|
|
||||||
// Heading error PIDF coefficients
|
// Heading error PIDF coefficients
|
||||||
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||||
2,
|
2,
|
||||||
0,
|
0,
|
||||||
.025,
|
0.0375,
|
||||||
0);
|
0);
|
||||||
|
|
||||||
// Feed forward constant added on to the heading PIDF
|
// Feed forward constant added on to the heading PIDF
|
||||||
@ -69,9 +69,9 @@ public class FollowerConstants {
|
|||||||
|
|
||||||
// Drive PIDF coefficients
|
// Drive PIDF coefficients
|
||||||
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
|
||||||
0.006,
|
0.00375,
|
||||||
0,
|
0,
|
||||||
0.00001,
|
0.00003,
|
||||||
0.8,
|
0.8,
|
||||||
0);
|
0);
|
||||||
|
|
||||||
|
|||||||
@ -93,11 +93,13 @@ public class ForwardVelocityTuner extends OpMode {
|
|||||||
}
|
}
|
||||||
|
|
||||||
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
|
// telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
|
||||||
telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
|
// telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
|
||||||
telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
|
// telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
|
||||||
telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
|
// telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
|
||||||
telemetryA.update();
|
//
|
||||||
|
//
|
||||||
|
// telemetryA.update();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -138,6 +140,13 @@ public class ForwardVelocityTuner extends OpMode {
|
|||||||
velocities.add(currentVelocity);
|
velocities.add(currentVelocity);
|
||||||
velocities.remove(0);
|
velocities.remove(0);
|
||||||
}
|
}
|
||||||
|
telemetryA.addData("x", poseUpdater.getPose().getX());
|
||||||
|
telemetryA.addData("y", poseUpdater.getPose().getY());
|
||||||
|
telemetryA.addData("heading", poseUpdater.getPose().getHeading());
|
||||||
|
telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||||
|
telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
|
||||||
|
telemetryA.update();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
double average = 0;
|
double average = 0;
|
||||||
for (Double velocity : velocities) {
|
for (Double velocity : velocities) {
|
||||||
@ -146,7 +155,12 @@ public class ForwardVelocityTuner extends OpMode {
|
|||||||
average /= (double) velocities.size();
|
average /= (double) velocities.size();
|
||||||
|
|
||||||
telemetryA.addData("forward velocity:", average);
|
telemetryA.addData("forward velocity:", average);
|
||||||
telemetryA.update();
|
// telemetryA.addData("x", poseUpdater.getPose().getX());
|
||||||
|
// telemetryA.addData("y", poseUpdater.getPose().getY());
|
||||||
|
// telemetryA.addData("heading", poseUpdater.getPose().getHeading());
|
||||||
|
// telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
||||||
|
// telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
|
||||||
|
// telemetryA.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -119,13 +119,6 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
|
|||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void loop() {
|
public void loop() {
|
||||||
|
|
||||||
telemetry.addData("x",poseUpdater.getPose().getX());
|
|
||||||
telemetry.addData("y",poseUpdater.getPose().getY());
|
|
||||||
telemetry.addData("heading",poseUpdater.getPose().getHeading());
|
|
||||||
telemetry.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
|
|
||||||
telemetry.addData("velo theta", poseUpdater.getVelocity().getTheta());
|
|
||||||
|
|
||||||
if (gamepad1.cross || gamepad1.a) {
|
if (gamepad1.cross || gamepad1.a) {
|
||||||
requestOpModeStop();
|
requestOpModeStop();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -0,0 +1,18 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.states;
|
||||||
|
|
||||||
|
public class FieldStates {
|
||||||
|
|
||||||
|
public enum FieldLocation {
|
||||||
|
BUCKET, SUBMARINE, FLOATING, TRAVELING
|
||||||
|
}
|
||||||
|
|
||||||
|
private FieldLocation fieldLocation;
|
||||||
|
|
||||||
|
public FieldLocation getFieldLocation() {
|
||||||
|
return fieldLocation;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setFieldLocation(FieldLocation fieldLocation) {
|
||||||
|
this.fieldLocation = fieldLocation;
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,131 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.subsystem;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
public class DualMotorSliderSubsystem {
|
||||||
|
|
||||||
|
/*
|
||||||
|
liftSlideLeft - Left Motor for Dual Linear Slide as a DcMotorEx object
|
||||||
|
Currently, the value of (liftSlideLeft) is null because we haven't assigned a value (object) to it yet.
|
||||||
|
It expects an object of type "DcMotorEx".
|
||||||
|
*/
|
||||||
|
private DcMotorEx liftSlideLeft;
|
||||||
|
|
||||||
|
/*
|
||||||
|
liftSlideRight - Right Motor for Dual Linear Slide as a DcMotorEx object
|
||||||
|
Currently, the value of (liftSlideRight) is null because we haven't assigned a value (object) to it yet.
|
||||||
|
It expects an object of type "DcMotorEx".
|
||||||
|
*/
|
||||||
|
private DcMotorEx liftSlideRight;
|
||||||
|
|
||||||
|
/*
|
||||||
|
targetPosition - Variable that holds target position of slides.
|
||||||
|
*/
|
||||||
|
private int targetPosition = 0;
|
||||||
|
|
||||||
|
/*
|
||||||
|
getTargetPosition/setTargetPosition - Best practice to "hide" (private) targetPosition and,
|
||||||
|
instead, use a "setter" to set the target position value or
|
||||||
|
a "getter" to get the target position value.
|
||||||
|
*/
|
||||||
|
public void setTargetPosition(int value) {
|
||||||
|
targetPosition = value;
|
||||||
|
}
|
||||||
|
private int getTargetPosition() { return targetPosition; }
|
||||||
|
|
||||||
|
/*
|
||||||
|
PID - Proportional/Integral/Derivative Values
|
||||||
|
|
||||||
|
For a dual motor linear slide, we only tune the P - Proportion.
|
||||||
|
The Proportion variable (kp) answers the question "how fast do we want to get to our destination?"
|
||||||
|
|
||||||
|
It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
|
||||||
|
Since we're going straight, we don't need to worry about.
|
||||||
|
*/
|
||||||
|
public final static double kp = 0.0015, ki = 0, kd = 0;
|
||||||
|
|
||||||
|
/*
|
||||||
|
lastError/integralSum/timer - These 3 variables are placeholders in determining how much
|
||||||
|
power to send to both motors.
|
||||||
|
*/
|
||||||
|
private double lastError = 0;
|
||||||
|
private double integralSum = 0;
|
||||||
|
private ElapsedTime timer = new ElapsedTime();
|
||||||
|
|
||||||
|
public DualMotorSliderSubsystem(HardwareMap hardwareMap) {
|
||||||
|
/*
|
||||||
|
liftSlideLeft/liftSlideRight - Now, we are assigning a value of DcMotorEx to each variable
|
||||||
|
|
||||||
|
We "assign" the object DcMotorEx to liftSlideLeft and liftSlideRight and "link them" to the
|
||||||
|
driver hub configuration name that matches the motor on the slide.
|
||||||
|
*/
|
||||||
|
liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
|
||||||
|
liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void init() {
|
||||||
|
/*
|
||||||
|
Initialize the motors with the following settings (assuming slide is at the very bottom position):
|
||||||
|
|
||||||
|
- Reset the encoders to be zero
|
||||||
|
- When the motor stops moving (zero power), brake. This means we can't move the motors, not even gravity.
|
||||||
|
- Sets the motor to run without the encoder. This doesn't mean we won't use the encoder values (because we will).
|
||||||
|
It just means to not FULLY depend on them, we will just prefer to use motor power instead.
|
||||||
|
*/
|
||||||
|
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
To calculate power, we send the targetPosition value (how high we want the slider motors to
|
||||||
|
be based on motor 'ticks') to calculatePower function.
|
||||||
|
|
||||||
|
For every loop that occurs, it will constantly calculate power. So long as we're far away from
|
||||||
|
our "target" position, we will get as much power as possible. The closer we get, the lower the
|
||||||
|
power we will receive.
|
||||||
|
*/
|
||||||
|
public void update() {
|
||||||
|
double power = calculatePower();
|
||||||
|
liftSlideLeft.setPower(power);
|
||||||
|
liftSlideRight.setPower(power);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
Calculating power - To calculate the power, we determine the proportion, derivative and
|
||||||
|
integral of our closed loop system.
|
||||||
|
|
||||||
|
For more information, please visit:
|
||||||
|
- Introduction to Closed Loop System:
|
||||||
|
- https://www.ctrlaltftc.com/introduction-to-closed-loop-control
|
||||||
|
- The PID controller:
|
||||||
|
- https://www.ctrlaltftc.com/the-pid-controller
|
||||||
|
*/
|
||||||
|
private double calculatePower() {
|
||||||
|
double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
|
||||||
|
integralSum += error * timer.seconds();
|
||||||
|
double derivative = (error - lastError) / timer.seconds();
|
||||||
|
lastError = error;
|
||||||
|
timer.reset();
|
||||||
|
return (error * kp) + (derivative * kd) + (integralSum * ki);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void toLowBucketPosition() {
|
||||||
|
setTargetPosition(1500);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void toHighBucketPosition() {
|
||||||
|
setTargetPosition(3000);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -0,0 +1,105 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.subsystem;
|
||||||
|
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
|
||||||
|
|
||||||
|
import androidx.annotation.NonNull;
|
||||||
|
|
||||||
|
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
|
||||||
|
import com.acmerobotics.roadrunner.Action;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||||
|
|
||||||
|
public class MotorsSubsystem {
|
||||||
|
|
||||||
|
public HardwareMap hardwareMap;
|
||||||
|
public Telemetry telemetry;
|
||||||
|
|
||||||
|
public DcMotor frontLeftMotor;
|
||||||
|
public DcMotor backLeftMotor;
|
||||||
|
public DcMotor frontRightMotor;
|
||||||
|
public DcMotor backRightMotor;
|
||||||
|
|
||||||
|
public enum TravelState {
|
||||||
|
STOPPED, MOVING
|
||||||
|
}
|
||||||
|
|
||||||
|
public TravelState travelState;
|
||||||
|
|
||||||
|
public double power;
|
||||||
|
|
||||||
|
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
|
||||||
|
this.hardwareMap = hardwareMap;
|
||||||
|
this.telemetry = telemetry;
|
||||||
|
this.power = MAX_POWER;
|
||||||
|
}
|
||||||
|
|
||||||
|
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
|
||||||
|
this.hardwareMap = hardwareMap;
|
||||||
|
this.telemetry = telemetry;
|
||||||
|
this.power = power;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void init() {
|
||||||
|
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
|
||||||
|
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
|
||||||
|
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
||||||
|
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
|
||||||
|
|
||||||
|
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
|
||||||
|
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
|
||||||
|
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
||||||
|
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
||||||
|
|
||||||
|
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
|
this.setState(TravelState.STOPPED);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setFrontLeftMotorPower(double power) {
|
||||||
|
frontLeftMotor.setPower(power);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setBackLeftMotorPower(double power) {
|
||||||
|
backLeftMotor.setPower(power);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setFrontRightMotorPower(double power) {
|
||||||
|
frontRightMotor.setPower(power);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setBackRightMotorPower(double power) {
|
||||||
|
backRightMotor.setPower(power);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public void setState(TravelState travelState) {
|
||||||
|
this.travelState = travelState;
|
||||||
|
}
|
||||||
|
|
||||||
|
public TravelState getState() {
|
||||||
|
return this.travelState;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setPower(DcMotor motor, double power) {
|
||||||
|
motor.setPower(power);
|
||||||
|
if (power < 0.05) {
|
||||||
|
this.setState(TravelState.MOVING);
|
||||||
|
} else {
|
||||||
|
this.setState(TravelState.STOPPED);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -16,6 +16,9 @@ dependencies {
|
|||||||
implementation 'org.firstinspires.ftc:Vision:10.1.0'
|
implementation 'org.firstinspires.ftc:Vision:10.1.0'
|
||||||
implementation 'androidx.appcompat:appcompat:1.2.0'
|
implementation 'androidx.appcompat:appcompat:1.2.0'
|
||||||
|
|
||||||
implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
|
implementation "com.acmerobotics.roadrunner:ftc:0.1.14"
|
||||||
|
implementation "com.acmerobotics.roadrunner:core:1.0.0"
|
||||||
|
implementation "com.acmerobotics.roadrunner:actions:1.0.0"
|
||||||
|
implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user