Merge pull request FIRST-Tech-Challenge#79 from blazeboy75/jackrevoy/…

…teleop-start Jackrevoy/teleop start
IsaccBarker · Nov 26, 2021 · c0d4c96 · c0d4c96
2 parents ea38887 + 520b0a6
commit c0d4c96
Showing 1 changed file with 52 additions and 23 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java
@@ -28,20 +28,12 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.util.ElapsedTime;
 import com.qualcomm.robotcore.util.Range;
 
-//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-//import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.util.ElapsedTime;
-import com.qualcomm.robotcore.util.Range;
 
 /** This file contains an example of an iterative (Non-Linear) "OpMode".
  * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
@@ -55,8 +47,10 @@
  * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
  * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. */
 
-@TeleOp(name="Basic: Iterative OpMode", group="Iterative Opmode")
-//
+
+@TeleOp(name="Starter TeleOp", group="Iterative Opmode")
+
+
 // @Disabled
 public class StarterTeleOp extends OpMode
 {
@@ -67,6 +61,7 @@ public class StarterTeleOp extends OpMode
     private DcMotor backL = null;
     private DcMotor backR = null;
 
+
     /** Code to run ONCE when the driver hits INIT. */
     @Override
     public void init() {
@@ -80,6 +75,14 @@ public void init() {
         backL  = hardwareMap.get(DcMotor.class, "Back Left");
         backR = hardwareMap.get(DcMotor.class, "Back Right");
 
+
+        /** Sets the motors to run using encoders. */
+        frontL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+
         /** Most robots need the motor on one side to be reversed to drive forward.
         * Reverse the motor that runs backwards when connected directly to the battery. */
         frontL.setDirection(DcMotor.Direction.FORWARD);
@@ -91,43 +94,69 @@ public void init() {
         telemetry.addData("Status", "Initialized");
     }
 
+
     /** Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY. */
     @Override
     public void init_loop() {
     }
 
+
     /** Code to run ONCE when the driver hits PLAY. */
     @Override
     public void start() {
         runtime.reset();
     }
 
+
     /** Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP. */
     @Override
     public void loop() {
         /** Setup a variable for each drive wheel to save power level for telemetry. */
-        double leftPower;
-        double rightPower;
+        double leftFPower ;
+        double rightFPower;
+        double leftBPower ;
+        double rightBPower;
+
 
         /**POV Mode uses right stick to go forward, and left stick to turn
          *  will be combined in later release.
          *  This uses basic math to combine motions and is easier to drive straight. */
-        double drive = -gamepad1.left_stick_x;
-        double turn  =  gamepad1.right_stick_y;
-        leftPower    = Range.clip(drive + turn, -1.0, 1.0) ;
-        rightPower   = Range.clip(drive - turn, -1.0, 1.0) ;
-
-
+        double drive = -gamepad1.right_stick_y;
+        double turn  =  gamepad1.right_stick_x;
+        double strafe = gamepad1.left_stick_x;
+
+
+         if (strafe !=0){
+            /** Untested on the robot. May need to be reversed or changed entirely. */
+             leftFPower = -strafe;
+             rightFPower = strafe;
+             leftBPower =  strafe;
+             rightBPower = -strafe;
+        }
+
+        else if(drive !=0 || turn !=0) {
+          leftFPower = Range.clip(drive + turn, -1.0, 1.0);
+            rightFPower = Range.clip(drive - turn, -1.0, 1.0);
+            leftBPower = Range.clip(drive + turn, -1.0, 1.0);
+            rightBPower = Range.clip(drive - turn, -1.0, 1.0);
+        }
+
+        else{
+            leftFPower = 0;
+            rightFPower = 0;
+            leftBPower = 0;
+            rightBPower = 0;
+        }
 
         /**Send calculated power to wheels. */
-        frontL.setPower(leftPower);
-        backL.setPower(leftPower);
-        frontR.setPower(rightPower);
-        backR.setPower(rightPower);
+        frontL.setPower(leftFPower);
+        backL.setPower(leftBPower);
+        frontR.setPower(rightFPower);
+        backR.setPower(rightBPower);
 
         /**  Show the elapsed game time and wheel power. */
         telemetry.addData("Status", "Run Time: " + runtime.toString());
-        telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);
+        telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftFPower, rightFPower);
     }
 
     /** Code to run ONCE after the driver hits STOP. */