tuning psi bot

svhsrobotics · Feb 9, 2024 · b65229f · b65229f
1 parent be53ef0
commit b65229f
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 2 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java
@@ -60,7 +60,7 @@ public static double getMotorVelocityF(double ticksPerSecond) {
     public static double DW_WHEEL_RADIUS = 0.944882; // in
     public static double DW_GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
 
-    public static double LATERAL_DISTANCE = 8.128; // in; distance between the left and right wheels
+    public static double LATERAL_DISTANCE = 8.136; // in; distance between the left and right wheels
     //public static double FORWARD_OFFSET = -6.5; // in; offset of the lateral wheel
     public static double FORWARD_OFFSET = -7.5;
 

diff --git a/...n/java/org/firstinspires/ftc/teamcode/drive/tuning/TrackingWheelLateralDistanceTuner.java b/...n/java/org/firstinspires/ftc/teamcode/drive/tuning/TrackingWheelLateralDistanceTuner.java
@@ -7,6 +7,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.util.RobotLog;
 
+import org.firstinspires.ftc.teamcode.drive.PsiBot;
 import org.firstinspires.ftc.teamcode.drive.Robot;
 import org.firstinspires.ftc.teamcode.drive.RoboticaBot;
 import org.firstinspires.ftc.teamcode.drive.TrackingWheelLocalizer;
@@ -125,7 +126,7 @@ public void runOpMode() throws InterruptedException {
         telemetry.clearAll();
         telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
         telemetry.addLine("Effective LATERAL_DISTANCE: " +
-                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * RoboticaBot.LATERAL_DISTANCE);
+                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * PsiBot.LATERAL_DISTANCE);
 
         telemetry.update();
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/HWTEST.java
@@ -6,6 +6,8 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.drive.Robot;
 import org.firstinspires.ftc.teamcode.drive.TrackingWheelLocalizer;
 import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
@@ -23,6 +25,9 @@ public void runOpMode() throws InterruptedException {
         Robot robot = Robot.thisRobot(hardwareMap);
         TrajectoryDrive drive = robot.getDrive();
         waitForStart();
+        int leftI = ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition();
+        int rightI = ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition();
+        int frontI = ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition();
         while (opModeIsActive()) {
             Vector2d input = new Vector2d(-gamepad1.left_stick_y, -gamepad1.left_stick_x);
             drive.setWeightedDrivePower(new Pose2d(input.getX(), input.getY(), -gamepad1.right_stick_x));
@@ -31,6 +36,19 @@ public void runOpMode() throws InterruptedException {
             android.util.Log.i("TESTODO", "LEFT " + ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition());
             android.util.Log.i("TESTODO", "RIGHT " + ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition());
             android.util.Log.i("TESTODO", "CENTER " + ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition());
+            telemetry.addData("LEFT", ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition());
+            telemetry.addData("RIGHT", ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition());
+            telemetry.addData("CENTER", ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition());
+
+            telemetry.addData("DELTA LEFT", ((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition() - leftI);
+            telemetry.addData("DELTA RIGHT", ((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition()- rightI);
+            telemetry.addData("DELTA CENTER", ((TrackingWheelLocalizer) drive.getLocalizer()).frontEncoder.getCurrentPosition() - frontI);
+
+            telemetry.addData("DELTA L-R", (((TrackingWheelLocalizer) drive.getLocalizer()).leftEncoder.getCurrentPosition() - leftI)- (((TrackingWheelLocalizer) drive.getLocalizer()).rightEncoder.getCurrentPosition()- rightI));
+
+            telemetry.update();
+
+