Finalize tunings
This commit is contained in:
@ -63,19 +63,19 @@ public class PedroConstants {
|
|||||||
public static final double ROBOT_WEIGHT_IN_KG = 9;
|
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.5;
|
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.7;
|
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.8;
|
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.7;
|
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,151 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.cometbots;
|
|
||||||
|
|
||||||
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 = "BlueNonBasketAuto", group = "Autonomous Pathing Tuning")
|
|
||||||
public class BlueNonBasketAuto extends OpMode {
|
|
||||||
private Telemetry telemetryA;
|
|
||||||
|
|
||||||
private Follower follower;
|
|
||||||
|
|
||||||
private PathChain path;
|
|
||||||
|
|
||||||
private final Pose startPose = new Pose(10.929, 55.446, 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(10.929, 55.446, Point.CARTESIAN),
|
|
||||||
new Point(42.429, 46.446, Point.CARTESIAN),
|
|
||||||
new Point(36.321, 38.089, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 2
|
|
||||||
new BezierLine(
|
|
||||||
new Point(36.321, 38.089, Point.CARTESIAN),
|
|
||||||
new Point(59.786, 36.643, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 3
|
|
||||||
new BezierLine(
|
|
||||||
new Point(59.786, 36.643, Point.CARTESIAN),
|
|
||||||
new Point(59.304, 24.750, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 4
|
|
||||||
new BezierLine(
|
|
||||||
new Point(59.304, 24.750, Point.CARTESIAN),
|
|
||||||
new Point(13.982, 23.946, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 5
|
|
||||||
new BezierLine(
|
|
||||||
new Point(13.982, 23.946, Point.CARTESIAN),
|
|
||||||
new Point(59.464, 24.429, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 6
|
|
||||||
new BezierLine(
|
|
||||||
new Point(59.464, 24.429, Point.CARTESIAN),
|
|
||||||
new Point(58.982, 15.268, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 7
|
|
||||||
new BezierLine(
|
|
||||||
new Point(58.982, 15.268, Point.CARTESIAN),
|
|
||||||
new Point(13.821, 14.464, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 8
|
|
||||||
new BezierLine(
|
|
||||||
new Point(13.821, 14.464, Point.CARTESIAN),
|
|
||||||
new Point(58.661, 13.500, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 9
|
|
||||||
new BezierLine(
|
|
||||||
new Point(58.661, 13.500, Point.CARTESIAN),
|
|
||||||
new Point(58.339, 8.679, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0))
|
|
||||||
.addPath(
|
|
||||||
// Line 10
|
|
||||||
new BezierLine(
|
|
||||||
new Point(58.339, 8.679, Point.CARTESIAN),
|
|
||||||
new Point(14.625, 8.518, Point.CARTESIAN)
|
|
||||||
)
|
|
||||||
)
|
|
||||||
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
|
|
||||||
follower.followPath(path, true);
|
|
||||||
|
|
||||||
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, true);
|
|
||||||
}
|
|
||||||
follower.telemetryDebug(telemetryA);
|
|
||||||
}
|
|
||||||
}
|
|
@ -57,9 +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.0029;//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 STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
public static double STRAFE_TICKS_TO_INCHES = -0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
|
||||||
public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5;
|
public static double TURN_TICKS_TO_RADIANS = 0.0029;//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)
|
||||||
@ -80,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.25, 0);
|
leftEncoderPose = new Pose(0.25, 6.25, 0);
|
||||||
rightEncoderPose = new Pose(0, -6.25, 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;
|
||||||
|
|
||||||
|
@ -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
|
||||||
@ -60,7 +60,7 @@ public class FollowerConstants {
|
|||||||
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
|
||||||
2,
|
2,
|
||||||
0,
|
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);
|
||||||
|
|
||||||
|
Reference in New Issue
Block a user