pid tuner and a bunch of stuff

pid tuner and a bunch of stuff

Author: RajDharkar

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/testers/DemoTeleOp.java

@@ -3,6 +3,9 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.firstinspires.ftc.teamcode.config.utils.ButtonToggle;
 
 @TeleOp
 public class DemoTeleOp extends LinearOpMode {
@@ -19,20 +22,36 @@ public void runOpMode() throws InterruptedException {
         // If your robot moves backwards when commanded to go forwards,
         // reverse the left side instead.
         // See the note about this earlier on this page.
-        //frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-        //backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-
+        frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
         waitForStart();
-
+        ButtonToggle a = new ButtonToggle();
+        ButtonToggle b = new ButtonToggle();
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
-            double y = Math.pow(-gamepad1.left_stick_y,3); // Remember, Y stick value is reversed
-            double x = Math.pow(gamepad1.left_stick_x * 1.1,3); // Counteract imperfect strafing
-            double rx = Math.pow(gamepad1.right_stick_x,3);
+
+            if(a.isClicked(gamepad1.a)) {
+                frontRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+                backRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+                frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+                backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+                telemetry.addData("left reverse", 1);
+            }
+            if(b.isClicked(gamepad1.b)){
+                frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+                backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+                frontLeftMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+                backLeftMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+                telemetry.addData("right reverse", 2);
+            }
+            double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
+            double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
+            double rx = gamepad1.right_stick_x;
 
             // Denominator is the largest motor power (absolute value) or 1
-            // This ensures all the powers maintain the same ratio,x
+            // This ensures all the powers maintain the same ratio,
             // but only if at least one is out of the range [-1, 1]
             double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
             double frontLeftPower = (y + x + rx) / denominator;
@@ -44,6 +63,8 @@ public void runOpMode() throws InterruptedException {
             backLeftMotor.setPower(backLeftPower);
             frontRightMotor.setPower(frontRightPower);
             backRightMotor.setPower(backRightPower);
+
+            telemetry.update();
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/config/testers/ServoTesters.java

@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.config.testers;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@TeleOp(group="Test")
+public class ServoTesters extends OpMode {
+    Servo leftArm;
+    Servo rightArm;
+
+    Servo leftClaw;
+    Servo rightClaw;
+
+    @Override
+    public void init() {
+        // Initialize the servos using hardwareMap and ensure they are named correctly in your configuration file
+        leftArm = hardwareMap.get(Servo.class, "leftArm");
+        rightArm = hardwareMap.get(Servo.class, "rightArm");
+
+        leftClaw = hardwareMap.get(Servo.class, "leftClaw");
+        rightClaw = hardwareMap.get(Servo.class, "rightClaw");
+
+
+    }
+
+    @Override
+    public void loop() {
+        // Use the gamepad to control the servos for testing
+        // Left joystick controls arms
+        double armPosition = (gamepad1.left_stick_y + 1) / 2; // Normalize joystick value to 0-1 range
+        leftArm.setPosition(armPosition); // Direct movement
+        rightArm.setPosition(1 - armPosition);
+
+        // Right joystick controls claws
+        double clawPosition = (gamepad1.right_stick_y + 1) / 2; // Normalize joystick value to 0-1 range
+        leftClaw.setPosition(clawPosition); // Direct movement
+        rightClaw.setPosition(1 - clawPosition); // Reverse movement
+
+        // Display positions on telemetry for debugging
+        telemetry.addData("Arm Positions", "Left: %.2f, Right: %.2f", armPosition, 1 - armPosition);
+        telemetry.addData("Claw Positions", "Left: %.2f, Right: %.2f", clawPosition, 1 - clawPosition);
+        telemetry.update();
+    }
+}

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

@@ -171,6 +171,7 @@ public void initialize() {
         // TODO: Make sure that this is the direction your motors need to be reversed in.
         leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
         leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        rightRear.setDirection(DcMotorSimple.Direction.REVERSE);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);