Merge remote-tracking branch 'origin/Branch-Yixuan' into Branch-Yixuan

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

@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+public class MovementLibrary {
+    public static void setDirectionForward(DcMotor rhw, DcMotor rlw, DcMotor lhw, DcMotor llw){
+        rhw.setDirection(DcMotor.Direction.REVERSE);
+        rlw.setDirection(DcMotor.Direction.FORWARD);
+        lhw.setDirection(DcMotorSimple.Direction.FORWARD);
+        llw.setDirection(DcMotorSimple.Direction.REVERSE);}
+
+    public static void setDirectionBackward(DcMotor rhw, DcMotor rlw, DcMotor lhw, DcMotor llw){
+        rhw.setDirection(DcMotor.Direction.FORWARD);
+        rlw.setDirection(DcMotor.Direction.REVERSE);
+        lhw.setDirection(DcMotorSimple.Direction.REVERSE);
+        llw.setDirection(DcMotorSimple.Direction.FORWARD);
+    }
+
+    public static void setSpeed (double speedValue,DcMotor rhw, DcMotor rlw, DcMotor lhw, DcMotor llw){
+        rhw.setPower(speedValue);
+        rlw.setPower(speedValue);
+        llw.setPower(speedValue);
+        lhw.setPower(speedValue);
+
+
+    }
+    public static void setDirectionRight(DcMotor rhw, DcMotor rlw, DcMotor lhw, DcMotor llw){
+        rhw.setDirection(DcMotor.Direction.REVERSE);
+        rlw.setDirection(DcMotor.Direction.REVERSE);
+        lhw.setDirection(DcMotorSimple.Direction.FORWARD);
+        llw.setDirection(DcMotorSimple.Direction.FORWARD);
+
+    }
+    public static void setDirectionLeft(DcMotor rhw, DcMotor rlw, DcMotor lhw, DcMotor llw){
+        rhw.setDirection(DcMotor.Direction.FORWARD);
+        rlw.setDirection(DcMotor.Direction.FORWARD);
+        lhw.setDirection(DcMotorSimple.Direction.REVERSE);
+        llw.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+}

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

@@ -0,0 +1,99 @@
+package org.firstinspires.ftc.teamcode;
+
+import static java.lang.Math.round;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.AnalogSensor;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.TouchSensor;
+import org.firstinspires.ftc.teamcode.MovementLibrary;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+import java.util.ArrayList;
+import java.util.List;
+
+@Autonomous(name="Name")
+public class Name extends LinearOpMode  {
+    private DcMotor rightHandWheel;
+    private DcMotor rightLegWheel;
+    private DcMotor leftHandWheel;
+    private DcMotor leftLegWheel;
+    private TouchSensor touchSensor;
+    private DistanceSensor distanceSensor;
+    private ColorSensor colorSensor;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        rightHandWheel = hardwareMap.get(DcMotor.class,"right hand wheel");
+        rightLegWheel = hardwareMap.get(DcMotor.class,"right leg wheel");
+        leftHandWheel = hardwareMap.get(DcMotor.class, "left hand wheel");
+        leftLegWheel = hardwareMap.get(DcMotor.class, "left leg wheel");
+        touchSensor = hardwareMap.get(TouchSensor.class, "touchsensor");
+        distanceSensor = hardwareMap.get(DistanceSensor.class , "distancesensor");
+        colorSensor= hardwareMap.get(ColorSensor.class , "colorsensor");
+
+        DcMotor[] motors = {rightHandWheel, leftHandWheel, rightLegWheel, leftLegWheel};
+
+        MovementLibrary.setDirectionForward(rightHandWheel, rightLegWheel, leftHandWheel,leftLegWheel);
+
+        //from this
+        DcMotor[] motors = {rightHandWheel, leftHandWheel, rightLegWheel, leftLegWheel};
+
+        //thismotors
+        MovementLibrary.setDirectionForward(motors);
+
+        // Wait for the game to start (driver presses {Play
+        waitForStart();
+
+
+        // Wait for the game to start (driver presses PLAY)
+        waitForStart();
+
+        while(opModeIsActive()){
+
+       /* telemetry.speak( "Oh see, you see" +
+                "By the dusk's late light" +
+                "What so proudly we rained" +
+                "At the twilight's last gleaming?" +
+                "Whose broad stripes and dark stars" +
+                "Through the perilous fight" +
+                "Under the ramparts we watched" +
+                "Were so gallantly, no, streaming?" +
+                "And the rockets' red glare" +
+                "The bombs contracting in air" +
+                "Gave proof through the night" +
+                "That our flag was not there" +
+                "O say, that star-spangled banner doesn't wave" +
+                "Over the land of the enslaved and the home of the cowardly");*/
+
+            MovementLibrary.setSpeed(0.2, rightHandWheel, rightLegWheel,leftHandWheel,leftLegWheel);
+
+            if(touchSensor.isPressed()) {
+                telemetry.speak("Ouchie that hurt me and my feelings");
+                telemetry.addData("was i triggered", touchSensor.isPressed());
+            }
+
+            double nicerValue = distanceSensor.getDistance(DistanceUnit.INCH);
+
+            telemetry.addData("you are this far (in inches) --> ", "%.2f", nicerValue);
+            telemetry.addData("green color value",colorSensor.green());
+            telemetry.addData("blue color value" ,colorSensor.blue());
+            telemetry.update();
+
+            if (colorSensor.blue() > 800 && colorSensor.green() > 1100) {
+                MovementLibrary.setSpeed(0.4, rightHandWheel, rightLegWheel,leftHandWheel,leftLegWheel);
+            }else {
+                MovementLibrary.setSpeed(0.2, rightHandWheel, rightLegWheel,leftHandWheel,leftLegWheel);
+            }
+        }
+
+    }
+
+}