diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index ebfc6a3..fda3f04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -20,8 +20,8 @@ public class PedroConstants { // Robot motor direction public static final Direction FRONT_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 BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; + public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.REVERSE; + public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.REVERSE; // Robot encoder direction public static final double FRONT_LEFT_MOTOR_ENCODER = Encoder.FORWARD; @@ -34,22 +34,22 @@ public class PedroConstants { */ // 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 - 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 - 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 - 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 - 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 - 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 */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index ad74928..9a4e3d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -6,9 +6,6 @@ import com.qualcomm.robotcore.hardware.IMU; 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.TwoWheelLocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index e0aa739..7de4983 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -1,19 +1,20 @@ 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_DIRECTION; 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_DIRECTION; 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_DIRECTION; 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_DIRECTION; 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.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -34,21 +35,21 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; */ @Config public class DriveEncoderLocalizer extends Localizer { - private HardwareMap hardwareMap; + private final HardwareMap hardwareMap; private Pose startPose; private Pose displacementPose; private Pose currentVelocity; private Matrix prevRotationMatrix; - private NanoTimer timer; + private final NanoTimer timer; private long deltaTimeNano; - private Encoder leftFront; - private Encoder rightFront; - private Encoder leftRear; - private Encoder rightRear; + private final Encoder leftFront; + private final Encoder rightFront; + private final Encoder leftRear; + private final 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 FORWARD_TICKS_TO_INCHES = -0.0058; + public static double STRAFE_TICKS_TO_INCHES = -0.0057; + public static double TURN_TICKS_TO_RADIANS = -0.0008675; public static double ROBOT_WIDTH = 1; public static double ROBOT_LENGTH = 1; @@ -66,7 +67,7 @@ public class DriveEncoderLocalizer extends Localizer { * 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 map the HardwareMap * @param setStartPose the Pose to start from */ public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { @@ -74,8 +75,18 @@ public class DriveEncoderLocalizer extends Localizer { leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR)); leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR)); - rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR)); - rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR)); + rightRear = 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 leftFront.setDirection(FRONT_LEFT_MOTOR_ENCODER); @@ -137,7 +148,7 @@ public class DriveEncoderLocalizer extends Localizer { * @param heading the rotation of the Matrix */ 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, 1, -Math.sin(heading)); prevRotationMatrix.set(1, 0, Math.sin(heading)); @@ -172,7 +183,7 @@ public class DriveEncoderLocalizer extends Localizer { Matrix globalDeltas; setPrevRotationMatrix(getPose().getHeading()); - Matrix transformation = new Matrix(3,3); + 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); @@ -222,13 +233,13 @@ public class DriveEncoderLocalizer extends Localizer { * @return returns a Matrix containing the robot relative movement. */ public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); + 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())); + 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())); + 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))); + returnMatrix.set(2, 0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH))); return returnMatrix; }