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

Added tuned values for Bobito

Browse Source
This commit is contained in:
Carlos Rivas
2024-10-22 22:46:04 -07:00
parent 1c4b3723c3
commit 6a125ff247
3 changed files with 41 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,8 +20,8 @@ public class PedroConstants {
// 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;
public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE; public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.REVERSE;
public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.REVERSE;
// Robot encoder direction // Robot encoder direction
public static final double FRONT_LEFT_MOTOR_ENCODER = Encoder.FORWARD; public static final double FRONT_LEFT_MOTOR_ENCODER = Encoder.FORWARD;
@ -34,22 +34,22 @@ public class PedroConstants {
*/ */
// 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 = 5.15;
// 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 = 57.1750;
// 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 = 52.775;
// 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 = -57.805; public static final double FORWARD_ZERO_POWER_ACCEL = -68.235;
// 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 = -111.025;
// 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 */

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -6,9 +6,6 @@ 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.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.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;

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java
View File

@ -1,19 +1,20 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; 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_LEFT_MOTOR_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; 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.BACK_RIGHT_MOTOR_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; 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_LEFT_MOTOR_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; 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.FRONT_RIGHT_MOTOR_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_ENCODER;
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.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
@ -34,21 +35,21 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
*/ */
@Config @Config
public class DriveEncoderLocalizer extends Localizer { public class DriveEncoderLocalizer extends Localizer {
private HardwareMap hardwareMap; private final HardwareMap hardwareMap;
private Pose startPose; private Pose startPose;
private Pose displacementPose; private Pose displacementPose;
private Pose currentVelocity; private Pose currentVelocity;
private Matrix prevRotationMatrix; private Matrix prevRotationMatrix;
private NanoTimer timer; private final NanoTimer timer;
private long deltaTimeNano; private long deltaTimeNano;
private Encoder leftFront; private final Encoder leftFront;
private Encoder rightFront; private final Encoder rightFront;
private Encoder leftRear; private final Encoder leftRear;
private Encoder rightRear; private final Encoder rightRear;
private double totalHeading; private double totalHeading;
public static double FORWARD_TICKS_TO_INCHES = -0.6308; public static double FORWARD_TICKS_TO_INCHES = -0.0058;
public static double STRAFE_TICKS_TO_INCHES = 46.4839; public static double STRAFE_TICKS_TO_INCHES = -0.0057;
public static double TURN_TICKS_TO_RADIANS = -0.002; public static double TURN_TICKS_TO_RADIANS = -0.0008675;
public static double ROBOT_WIDTH = 1; public static double ROBOT_WIDTH = 1;
public static double ROBOT_LENGTH = 1; public static double ROBOT_LENGTH = 1;
@ -74,8 +75,18 @@ public class DriveEncoderLocalizer extends Localizer {
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR)); leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR));
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR)); leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR));
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR)); rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR));
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR)); rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR));
DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION);
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
// TODO: reverse any encoders necessary // TODO: reverse any encoders necessary
leftFront.setDirection(FRONT_LEFT_MOTOR_ENCODER); leftFront.setDirection(FRONT_LEFT_MOTOR_ENCODER);
@ -137,7 +148,7 @@ public class DriveEncoderLocalizer extends Localizer {
* @param heading the rotation of the Matrix * @param heading the rotation of the Matrix
*/ */
public void setPrevRotationMatrix(double heading) { public void setPrevRotationMatrix(double heading) {
prevRotationMatrix = new Matrix(3,3); prevRotationMatrix = new Matrix(3, 3);
prevRotationMatrix.set(0, 0, Math.cos(heading)); prevRotationMatrix.set(0, 0, Math.cos(heading));
prevRotationMatrix.set(0, 1, -Math.sin(heading)); prevRotationMatrix.set(0, 1, -Math.sin(heading));
prevRotationMatrix.set(1, 0, Math.sin(heading)); prevRotationMatrix.set(1, 0, Math.sin(heading));
@ -172,7 +183,7 @@ public class DriveEncoderLocalizer extends Localizer {
Matrix globalDeltas; Matrix globalDeltas;
setPrevRotationMatrix(getPose().getHeading()); setPrevRotationMatrix(getPose().getHeading());
Matrix transformation = new Matrix(3,3); Matrix transformation = new Matrix(3, 3);
if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { 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, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
@ -222,13 +233,13 @@ public class DriveEncoderLocalizer extends Localizer {
* @return returns a Matrix containing the robot relative movement. * @return returns a Matrix containing the robot relative movement.
*/ */
public Matrix getRobotDeltas() { public Matrix getRobotDeltas() {
Matrix returnMatrix = new Matrix(3,1); Matrix returnMatrix = new Matrix(3, 1);
// x/forward movement // x/forward movement
returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition())); returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
//y/strafe movement //y/strafe movement
returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition())); returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
// theta/turning // theta/turning
returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH))); returnMatrix.set(2, 0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
return returnMatrix; return returnMatrix;
} }