Updated movement library to the updated orientation of the motors, added speed control via bumpers

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

@@ -33,6 +33,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 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 com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
@@ -79,6 +80,9 @@ public class CometBotsLinearOpMode extends LinearOpMode {
     @Override
     public void runOpMode() {
 
+        // Button A counter to cycle speeds
+        int cntBtnA = 1;
+
         // Initialize the hardware variables. Note that the strings used here must correspond
         // to the names assigned during the robot configuration step on the DS or RC devices.
         rhw = hardwareMap.get(DcMotor.class, "right hand wheel");
@@ -116,22 +120,22 @@ public class CometBotsLinearOpMode extends LinearOpMode {
 
             // Combine the joystick requests for each axis-motion to determine each wheel's power.
             // Set up a variable for each drive wheel to save the power level for telemetry.
-            double leftFrontPower  = axial + lateral + yaw;
+            double lftFrntPwr  = axial + lateral + yaw;
-            double rightFrontPower = axial - lateral - yaw;
+            double rtFrntPwr = axial - lateral - yaw;
-            double leftBackPower   = axial - lateral + yaw;
+            double ltBckPwr   = axial - lateral + yaw;
-            double rightBackPower  = axial + lateral - yaw;
+            double rtBckPwr  = axial + lateral - yaw;
 
             // Normalize the values so no wheel power exceeds 100%
             // This ensures that the robot maintains the desired motion.
-            max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
+            max = Math.max(Math.abs(lftFrntPwr), Math.abs(rtFrntPwr));
-            max = Math.max(max, Math.abs(leftBackPower));
+            max = Math.max(max, Math.abs(ltBckPwr));
-            max = Math.max(max, Math.abs(rightBackPower));
+            max = Math.max(max, Math.abs(rtBckPwr));
 
             if (max > 1.0) {
-                leftFrontPower  /= max;
+                lftFrntPwr  /= max;
-                rightFrontPower /= max;
+                rtFrntPwr /= max;
-                leftBackPower   /= max;
+                ltBckPwr   /= max;
-                rightBackPower  /= max;
+                rtBckPwr  /= max;
             }
 
             // This is test code:
@@ -145,22 +149,39 @@ public class CometBotsLinearOpMode extends LinearOpMode {
             // Once the correct motors move in the correct direction re-comment this code.
 
             /*
-            leftFrontPower  = gamepad1.x ? 1.0 : 0.0;  // X gamepad
+            lftFrntPwr  = gamepad1.x ? 1.0 : 0.0;  // X gamepad
-            leftBackPower   = gamepad1.a ? 1.0 : 0.0;  // A gamepad
+            ltBckPwr   = gamepad1.a ? 1.0 : 0.0;  // A gamepad
-            rightFrontPower = gamepad1.y ? 1.0 : 0.0;  // Y gamepad
+            rtFrntPwr = gamepad1.y ? 1.0 : 0.0;  // Y gamepad
-            rightBackPower  = gamepad1.b ? 1.0 : 0.0;  // B gamepad
+            rtBckPwr  = gamepad1.b ? 1.0 : 0.0;  // B gamepad
             */
 
+            lhw.setPower(lftFrntPwr/cntBtnA);
+            rhw.setPower(rtFrntPwr/cntBtnA);
+            llw.setPower(ltBckPwr/cntBtnA);
+            rlw.setPower(rtBckPwr/cntBtnA);
+
             // Send calculated power to wheels
-            lhw.setPower(leftFrontPower);
+            if(gamepad1.left_bumper) {
-            rhw.setPower(rightFrontPower);
+                sleep(175);
-            llw.setPower(leftBackPower);
+                cntBtnA--;
-            rlw.setPower(rightBackPower);
+                if(cntBtnA < 0) {
+                    cntBtnA = 0;
+                }
+            }
+
+            if(gamepad1.right_bumper) {
+                sleep(175);
+                cntBtnA++;
+                if(cntBtnA > 4) {
+                    cntBtnA = 4;
+                }
+            }
 
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());
-            telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
+            telemetry.addData("Front left/Right", "%4.2f, %4.2f", lftFrntPwr, rtFrntPwr);
-            telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+            telemetry.addData("Back  left/Right", "%4.2f, %4.2f", ltBckPwr, rtBckPwr);
+            telemetry.addData("Power Mode", cntBtnA);
             telemetry.update();
         }
     }}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java

@@ -12,21 +12,21 @@ public class MovementLibrary {
 
     public static void setLeftSideForward(DcMotor[] motors) {
         motors[2].setDirection(DcMotor.Direction.FORWARD);
-        motors[3].setDirection(DcMotor.Direction.REVERSE);
+        motors[3].setDirection(DcMotor.Direction.FORWARD);
     }
 
     public static void setRightSideForward(DcMotor[] motors) {
-        motors[0].setDirection(DcMotor.Direction.REVERSE);
+        motors[0].setDirection(DcMotor.Direction.FORWARD);
         motors[1].setDirection(DcMotor.Direction.FORWARD);
     }
 
     public static void setLeftSideBackward(DcMotor[] motors) {
         motors[2].setDirection(DcMotor.Direction.REVERSE);
-        motors[3].setDirection(DcMotor.Direction.FORWARD);
+        motors[3].setDirection(DcMotor.Direction.REVERSE);
     }
 
     public static void setRightSideBackward(DcMotor[] motors) {
-        motors[0].setDirection(DcMotor.Direction.FORWARD);
+        motors[0].setDirection(DcMotor.Direction.REVERSE);
         motors[1].setDirection(DcMotor.Direction.REVERSE);
     }