Latest configurations for black competition robot

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+
+public class PedroConstants {
+
+    /*
+        Robot parameters
+    */
+
+    // Robot motor configurations
+    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
+    public static final String BACK_LEFT_MOTOR  = "Drive back lt";
+    public static final String FRONT_RIGHT_MOTOR  = "Drive front rt";
+    public static final String BACK_RIGHT_MOTOR  = "Drive back rt";
+
+    // 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;
+
+    // Robot IMU configuration
+    public static final String IMU = "imu";
+
+    // Robot IMU placement
+    public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
+            = RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
+    public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
+            = RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
+
+    // Robot encoders
+    public static final String LEFT_ENCODER = "encoder left";
+    public static final String RIGHT_ENCODER = "encoder right";
+    public static final String BACK_ENCODER = "encoder back";
+
+    // Robot encoder direction
+    public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
+
+    /*
+        Pedro's parameters
+    */
+
+    // The weight of the robot in Kilograms
+    public static final double ROBOT_WEIGHT_IN_KG = 10.5;
+
+    // Maximum velocity of the robot going forward
+    public static final double ROBOT_SPEED_FORWARD = 72.0693;
+
+    // Maximum velocity of the robot going right
+    public static final double ROBOT_SPEED_LATERAL = 24.1401;
+
+    // Rate of deceleration when power is cut-off when the robot is moving forward
+    public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779;
+
+    // Rate of deceleration when power is cut-off when the robot is moving to the right
+    public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409;
+
+    // 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 = 4;
+
+    /* Centripetal force correction - increase if robot is correcting into the path
+                                    - decrease if robot is correcting away from the path */
+    public static final double CENTRIPETAL_SCALING = 0.0005;
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedrosConstants.java

@@ -1,44 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-public class PedrosConstants {
-
-    // Robot motor configurations
-    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
-    public static final String BACK_LEFT_MOTOR  = "Drive back lt";
-    public static final String FRONT_RIGHT_MOTOR  = "Drive front rt";
-    public static final String BACK_RIGHT_MOTOR  = "Drive back rt";
-
-    // Robot IMU configuration
-    public static final String IMU = "imu";
-
-    // Robot Encoders
-    public static final String LEFT_ENCODER = "encoder left";
-    public static final String RIGHT_ENCODER = "encoder right";
-    public static final String BACK_ENCODER = "encoder back";
-
-    /*
-        Pedro's parameters
-     */
-
-    // The weight of the robot in Kilograms
-    public static final double ROBOT_WEIGHT_IN_KG = 10.65942;
-
-    // Maximum velocity of the robot going forward
-    public static final double ROBOT_SPEED_FORWARD = 81.34056;
-
-    // Maximum velocity of the robot going right
-    public static final double ROBOT_SPEED_LATERAL = 65.43028;
-
-    // Rate of deceleration when power is cut-off when the robot is moving forward
-    public static final double FORWARD_ZERO_POWER_ACCEL = -34.62719;
-
-    // Rate of deceleration when power is cut-off when the robot is moving to the right
-    public static final double LATERAL_ZERO_POWER_ACCEL = -78.15554;
-
-    // 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 = 4;
-
-    /* Centripetal force correction - increase if robot is correcting into the path
-                                    - decrease if robot is correcting away from the path */
-    public static final double CENTRIPETAL_SCALING = 0.0005;
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.follower;
 
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
@@ -168,8 +169,10 @@ public class Follower {
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
 
         // TODO: Make sure that this is the direction your motors need to be reversed in.
-        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);
 

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

@@ -69,7 +69,7 @@ public class PoseUpdater {
      */
     public PoseUpdater(HardwareMap hardwareMap) {
         // TODO: replace the second argument with your preferred localizer
-        this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
+        this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
@@ -65,9 +65,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
     private double previousIMUOrientation;
     private double deltaRadians;
     private double totalHeading;
-    public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
+    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.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
+    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.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5;
+    public static double TURN_TICKS_TO_RADIANS = 0.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5;
 
     public static boolean useIMU = true;
 
@@ -93,12 +93,12 @@ public class ThreeWheelIMULocalizer extends Localizer {
         imu = hardwareMap.get(IMU.class, IMU);
 
         // TODO: replace this with your IMU's orientation
-        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP)));
+        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(-3, 5.7, 0);
+        leftEncoderPose = new Pose(-7.625, 6.19375, 0);
-        rightEncoderPose = new Pose(-3, -5.7, 0);
+        rightEncoderPose = new Pose(-7.625, -6.19375, 0);
-        strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90));
+        strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
 
         // TODO: replace these with your encoder ports
         leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
@@ -106,9 +106,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
         strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
 
         // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(Encoder.REVERSE);
+        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
-        rightEncoder.setDirection(Encoder.FORWARD);
+        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
-        strafeEncoder.setDirection(Encoder.FORWARD);
+        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();

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

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -92,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
         strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
 
         // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(Encoder.REVERSE);
+        //leftEncoder.setDirection(Encoder.REVERSE);
-        rightEncoder.setDirection(Encoder.REVERSE);
+       // rightEncoder.setDirection(Encoder.REVERSE);
-        strafeEncoder.setDirection(Encoder.FORWARD);
+        //strafeEncoder.setDirection(Encoder.FORWARD);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java

@@ -1,7 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.BACK_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedrosConstants.LEFT_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;

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

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.tuning;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java

@@ -52,7 +52,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
 
     private PoseUpdater poseUpdater;
 
-    public static double VELOCITY = 30;
+    public static double VELOCITY = 50;
 
     private double previousVelocity;