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

Finalize tunings

Browse Source
This commit is contained in:
Carlos Rivas
2024-12-18 20:42:14 -08:00
parent c9ffd4f061
commit 3e79d86443
4 changed files with 16 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
View File

@ -63,19 +63,19 @@ public class PedroConstants {
public static final double ROBOT_WEIGHT_IN_KG = 9;
// 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
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
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
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
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
- decrease if robot is correcting away from the path */

151
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java
View File

@ -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);
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java
View File

@ -57,9 +57,9 @@ public class ThreeWheelLocalizer extends Localizer {
private Pose rightEncoderPose;
private Pose strafeEncoderPose;
private double totalHeading;
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.0029;//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 FORWARD_TICKS_TO_INCHES = 0.003;//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 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)
@ -80,9 +80,9 @@ public class ThreeWheelLocalizer extends Localizer {
*/
public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
// TODO: replace these with your encoder positions
leftEncoderPose = new Pose(0, 6.25, 0);
rightEncoderPose = new Pose(0, -6.25, 0);
strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90));
leftEncoderPose = new Pose(0.25, 6.25, 0);
rightEncoderPose = new Pose(0.25, -6.25, 0);
strafeEncoderPose = new Pose(-7, 0.25, Math.toRadians(90));
hardwareMap = map;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java
View File

@ -40,9 +40,9 @@ public class FollowerConstants {
// Translational PIDF coefficients (don't use integral)
public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
0.1,
.25,
0,
0.01,
0.0375,
0);
// Translational Integral
@ -60,7 +60,7 @@ public class FollowerConstants {
public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
2,
0,
0.025,
0.0375,
0);
// Feed forward constant added on to the heading PIDF
@ -69,9 +69,9 @@ public class FollowerConstants {
// Drive PIDF coefficients
public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
0.006,
0.00375,
0,
0.00001,
0.00003,
0.8,
0);