helloworldhellow

IMU.java

@@ -0,0 +1,184 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+
+import com.qualcomm.robotcore.hardware.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@TeleOp(name="Yale Holo", group="WeRobot")
+public class YaleHolo extends LinearOpMode {
+    
+    float rotate_angle = 0;
+    double reset_angle = 0;
+    
+    private DcMotor front_left_wheel = null;
+    private DcMotor back_left_wheel = null;
+    private DcMotor back_right_wheel = null;
+    private DcMotor front_right_wheel = null;
+    
+    IMU imu;
+    @Override
+    public void runOpMode() {
+        front_left_wheel = hardwareMap.dcMotor.get("flm");
+        back_left_wheel = hardwareMap.dcMotor.get("blm");
+        back_right_wheel = hardwareMap.dcMotor.get("brm");
+        front_right_wheel = hardwareMap.dcMotor.get("frm");
+        
+        front_left_wheel.setDirection(DcMotor.Direction.REVERSE); 
+        back_left_wheel.setDirection(DcMotor.Direction.REVERSE);
+        front_right_wheel.setDirection(DcMotor.Direction.FORWARD);
+        back_right_wheel.setDirection(DcMotor.Direction.FORWARD);
+        
+        front_left_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        back_left_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        front_right_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        back_right_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        
+        imu = hardwareMap.get(IMU.class, "imu");
+        
+        imu.initialize(
+            new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                    RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
+                )
+            )
+            );
+        imu.resetYaw();
+        
+        while(!opModeIsActive()){}
+        
+        while(opModeIsActive()){
+            drive();
+            resetAngle();
+            //driveSimple();
+            telemetry.update();
+        }
+    }
+    public void driveSimple(){
+        double power = .5;
+        if(gamepad1.dpad_up){ //Forward
+            front_left_wheel.setPower(-power);
+            back_left_wheel.setPower(-power);
+            back_right_wheel.setPower(-power);
+            front_right_wheel.setPower(-power);
+        }
+        else if(gamepad1.dpad_left){ //Left
+            front_left_wheel.setPower(power);
+            back_left_wheel.setPower(-power);
+            back_right_wheel.setPower(power);
+            front_right_wheel.setPower(-power);
+        }
+        else if(gamepad1.dpad_down){ //Back
+            front_left_wheel.setPower(power);
+            back_left_wheel.setPower(power);
+            back_right_wheel.setPower(power);
+            front_right_wheel.setPower(power);
+        }
+        else if(gamepad1.dpad_right){ //Right
+            front_left_wheel.setPower(-power);
+            back_left_wheel.setPower(power);
+            back_right_wheel.setPower(-power);
+            front_right_wheel.setPower(power);
+        }
+        else if(Math.abs(gamepad1.right_stick_x) > 0){ //Rotation
+            front_left_wheel.setPower(-gamepad1.right_stick_x);
+            back_left_wheel.setPower(-gamepad1.right_stick_x);
+            back_right_wheel.setPower(gamepad1.right_stick_x);
+            front_right_wheel.setPower(gamepad1.right_stick_x);
+        }
+        else{
+            front_left_wheel.setPower(0);
+            back_left_wheel.setPower(0);
+            back_right_wheel.setPower(0);
+            front_right_wheel.setPower(0);
+        }
+    }
+    public void drive() {
+        double Protate = gamepad1.right_stick_x/4;
+        double stick_x = gamepad1.left_stick_x * Math.sqrt(Math.pow(1-Math.abs(Protate), 2)/2); //Accounts for Protate when limiting magnitude to be less than 1
+        double stick_y = gamepad1.left_stick_y * Math.sqrt(Math.pow(1-Math.abs(Protate), 2)/2);
+        double theta = 0;
+        double Px = 0;
+        double Py = 0;
+
+        double gyroAngle = getHeading() * Math.PI / 180; //Converts gyroAngle into radians
+        if (gyroAngle <= 0) { 
+            gyroAngle = gyroAngle + (Math.PI / 2);
+        } else if (0 < gyroAngle && gyroAngle < Math.PI / 2) {
+            gyroAngle = gyroAngle + (Math.PI / 2);
+        } else if (Math.PI / 2 <= gyroAngle) {
+            gyroAngle = gyroAngle - (3 * Math.PI / 2);
+        }
+        gyroAngle = -1 * gyroAngle;
+        
+        if(gamepad1.right_bumper){ //Disables gyro, sets to -Math.PI/2 so front is defined correctly. 
+            gyroAngle = -Math.PI/2;
+        }
+        
+        //Linear directions in case you want to do straight lines.
+        if(gamepad1.dpad_right){
+            stick_x = 0.5;
+        }
+        else if(gamepad1.dpad_left){
+            stick_x = -0.5;
+        }
+        if(gamepad1.dpad_up){
+            stick_y = -0.5;
+        }
+        else if(gamepad1.dpad_down){
+            stick_y = 0.5;
+        }
+        
+        
+        //MOVEMENT
+        theta = Math.atan2(stick_y, stick_x) - gyroAngle - (Math.PI / 2);
+        Px = Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)) * (Math.sin(theta + Math.PI / 4));
+        Py = Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)) * (Math.sin(theta - Math.PI / 4));
+        
+        telemetry.addData("Stick_X", stick_x);
+        telemetry.addData("Stick_Y", stick_y);
+        telemetry.addData("Magnitude",  Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)));
+        telemetry.addData("Front Left", Py - Protate);
+        telemetry.addData("Back Left", Px - Protate);
+        telemetry.addData("Back Right", Py + Protate);
+        telemetry.addData("Front Right", Px + Protate);
+        
+        front_left_wheel.setPower(Py - Protate);
+        back_left_wheel.setPower(Px - Protate);
+        back_right_wheel.setPower(Py + Protate);
+        front_right_wheel.setPower(Px + Protate);
+    }
+    public void resetAngle(){
+        if(gamepad1.a){
+            reset_angle = getHeading() + reset_angle;
+        }
+    }
+    public double getHeading(){
+        
+        
+        double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+        if(heading < -180) {
+            heading = heading + 360;
+        }
+        else if(heading > 180){
+            heading = heading - 360;
+        }
+        heading = heading - reset_angle;
+        return heading;
+    }
+}
+    

autonome_api.java

@@ -0,0 +1,111 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import com.qualcomm.robotcore.hardware.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@Autonomous
+
+public class ftc2024_autonome_api extends LinearOpMode {
+    private DcMotor rm;
+    private DcMotor lm;
+    private DcMotor harvestmotor;
+    private IMU imu;
+    private final double wheel_width = 9.0e-2; // metres
+    private final double wheel_perimeter = Math.PI * wheel_width;
+    private final double tour_par_minutes = 300.0;
+    private final double ground_tiles_width = 61.0e-2; // metres
+    /*
+     * return a metre/sec speed
+     * @param motor_speed = double between 0 and 1
+     */
+    public double getSpeedFromMotorSpeed(double motor_speed = 1.0){
+      double speed_tour_par_minutes = this.tour_par_minutes*motor_speed;
+      double speed = (speed_tour_par_minutes/60)*this.wheel_perimeter;
+      return speed;
+    }
+    public double time_for_dist(double dist, double motor_speed=1.0){
+      double speed = getSpeedFromMotorSpeed(motor_speed);
+      return (dist/speed);
+    }
+    public void forward(double n_tiles, double motor_speed = 1.0){
+      double total_time = time_for_dist(n_tiles*ground_tiles_width, motor_speed);
+      double start_time = runtime.seconds();
+      while( opModeIsActive() && ((runtime.seconds()-start_time)<total_time)){
+        lm.setPower(motor_speed);
+        rm.setPower(motor_speed);
+      }
+      lm.setPower(0);
+      rm.setPower(0);
+    }
+    public void backward(double n_tiles, double motor_speed= 1.0){
+        forward(n_tiles, -motor_speed);
+    }
+    public void harvest(double motor_speed=0.0){
+        harvestmotor.setPower(motor_speed);
+    }
+    public void rotate(double angle, double motor_speed=1.0){
+        double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+        angle = 200.0;
+        double anglerad = Math.toRadians(angle);
+        angle = Math.toDegrees(Math.atan2(Math.sin(anglerad),Math.cos(anglerad)));
+        double left_multiplier = -( (double) Math.signum(angle));
+        double right_multiplier = ((double) Math.signum(angle));
+        double m_power = motor_speed;
+        while(opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle)){
+            m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative 
+            lm.setPower(left_multiplier*m_power);
+            rm.setPower(right_multiplier*m_power);
+        }
+        lm.setPower(0);
+        rm.setPower(0);
+    }
+
+    @Override
+
+    public void runOpMode() {
+        lm = hardwareMap.get (DcMotor.class, "blm");
+        rm = hardwareMap.get (DcMotor.class, "brm");
+        harvestmotor = hardwareMap.get(DcMotor.class, "flm");
+
+        rm.setDirection(DcMotor.Direction.REVERSE);
+        
+        imu = hardwareMap.get(IMU.class, "imu");
+        imu.initialize(
+            new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                    RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
+                )
+            )
+            );
+        imu.resetYaw();
+        YawPitchRollAngles robotOrientation;
+        robotOrientation = imu.getRobotYawPitchRollAngles();
+        double Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
+        double yaw_sortie = 0.0;
+
+        waitForStart();
+
+        if(opModeIsRunning()){
+            forward(0.5);
+            rotate(-90.0);
+            forward(1.5);
+            harvest(-1);
+            backward(1);
+            harvest(0);
+    }
+}

ftc2024-autonome.java

@@ -1,36 +1,29 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.AnalogInput;
-import com.qualcomm.robotcore.hardware.Gyroscope;
-import com.qualcomm.robotcore.hardware.ColorSensor;
-import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.util.ElapsedTime;
 
 import com.qualcomm.robotcore.hardware.IMU;
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
 import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
 import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
 import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
-import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Position;
-import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
 
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
 
 @Autonomous
 
-public class ftc2024_autonome extends LinearOpMode {
+public class ftc2024_autonome_test extends LinearOpMode {
     private DcMotor rm;
     private DcMotor lm;
     private IMU imu;
+<<<<<<< HEAD
     private ElapsedTime     runtime = new ElapsedTime();
     YawPitchRollAngles robotOrientation;
     
@@ -60,11 +53,16 @@ public class ftc2024_autonome extends LinearOpMode {
     double clamp(double value, double max) {
         return Math.max(-max, Math.min(max, value));
     }
+=======
+>>>>>>> 7cd7be1f14eec5f1be12b9f60543771de58ca782
 
     @Override
+
     public void runOpMode() {
-        lm = hardwareMap.get(DcMotor.class, "blm");
+        lm = hardwareMap.get (DcMotor.class, "blm");
-        rm = hardwareMap.get(DcMotor.class, "brm");
+        rm = hardwareMap.get (DcMotor.class, "brm");
+
+        rm.setDirection(DcMotor.Direction.REVERSE);
         
         imu = hardwareMap.get(IMU.class, "imu");
         imu.initialize(
@@ -76,6 +74,7 @@ public class ftc2024_autonome extends LinearOpMode {
             )
             );
         imu.resetYaw();
+<<<<<<< HEAD
 
         rm.setDirection(DcMotorSimple.Direction.REVERSE);
         telemetry.addData("Status", "Initialized");
@@ -87,17 +86,16 @@ public class ftc2024_autonome extends LinearOpMode {
         double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second
         boolean mode = true;
         
+=======
+        YawPitchRollAngles robotOrientation;
+>>>>>>> 7cd7be1f14eec5f1be12b9f60543771de58ca782
         robotOrientation = imu.getRobotYawPitchRollAngles();
-        
         double Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
-        double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
+        double yaw_sortie = 0.0;
-        double Roll  = robotOrientation.getRoll(AngleUnit.DEGREES);
 
-		double yaw_sortie;
-        
-        // Wait for the game to start (driver presses PLAY)
         waitForStart();
 
+<<<<<<< HEAD
         runtime.reset();
         if (mode){
             //mode Elina
@@ -165,20 +163,44 @@ public class ftc2024_autonome extends LinearOpMode {
                     telemetry.addData("current_op_id",i);
 
                     
+=======
+        while (opModeIsActive()){
+            double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
+            double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1};
+            double [] x = new double[lm_p.length];
+            for(int i = 0; i< 9; i++){
+                while (opModeIsActive() && Yaw < 90){
+                    lm.setPower(lm_p[i]);
+                    rm.setPower(rm_p[i]);
+                    robotOrientation = imu.getRobotYawPitchRollAngles();
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    telemetry.addData("Yaw ", Yaw);
+>>>>>>> 7cd7be1f14eec5f1be12b9f60543771de58ca782
                     telemetry.update();
+                    yaw_sortie = Yaw;
                 }
+                telemetry.addData("Yaw sortie", yaw_sortie);
+                telemetry.update();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                x [i]= Yaw - yaw_sortie;
+                
+                imu.resetYaw();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                telemetry.addData("Yaw", Yaw);
+                telemetry.update();
+            }
+            while (opModeIsActive()){
+                telemetry.addData("0.1", x[0]);
+                telemetry.addData("0.2", x[1]);
+                telemetry.addData("0.3", x[2]);
+                telemetry.addData("0.4", x[3]);
+                telemetry.addData("0.5", x[4]);
+                telemetry.addData("0.6", x[5]);
+                telemetry.addData("0.7", x[6]);
+                telemetry.addData("0.8", x[7]);
+                telemetry.addData("0.9", x[8]);
+                telemetry.addData("1", x[9]);
             }
         }
-
-        // Now use these simple methods to extract each angle
-        // (Java type double) from the object you just created:
-        
-        telemetry.addData("yaw",Yaw);
-
-        telemetry.update();
-        // run until the end of the match (driver presses STOP
-
     }
-
+}
-}
-

helloworld.py.java

@@ -1,4 +1,7 @@
-String pythonCode = """abc = \"\"\"
+String pythonCode = "code in comment";
+/*
+```py
+abc = """
 booledan already_//CURR// = false;
 if(gamepad1.//CURR// && !already_//CURR//){
     already_//CURR// =! already_//CURR//;
@@ -8,7 +11,7 @@ if (!gamepad1.//CURR// && already_//CURR//){
     already_//CURR// =! already_//CURR//;
 }
 
-\"\"\";
+""";
 defg = "";
 
 hijk = "abcdefghijklmnopqrstuvwxyz";
@@ -17,4 +20,5 @@ for i in hijk:
     defg += abc.replace("//CURR//",i);
 
 print(defg);
-""";
+```
+*/

test.java

@@ -0,0 +1,102 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import com.qualcomm.robotcore.hardware.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@Autonomous
+
+public class ftc2024_autonome_test extends LinearOpMode {
+    private DcMotor rm;
+    private DcMotor lm;
+    private IMU imu;
+
+    @Override
+
+    public void runOpMode() {
+        lm = hardwareMap.get (DcMotor.class, "blm");
+        rm = hardwareMap.get (DcMotor.class, "brm");
+
+        rm.setDirection(DcMotor.Direction.REVERSE);
+        
+        imu = hardwareMap.get(IMU.class, "imu");
+        imu.initialize(
+            new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                    RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
+                )
+            )
+            );
+        imu.resetYaw();
+        YawPitchRollAngles robotOrientation;
+        robotOrientation = imu.getRobotYawPitchRollAngles();
+        double Yaw   = robotOrientation.getYaw(AngleUnit.DEGREES);
+        double yaw_sortie = 0.0;
+
+        waitForStart();
+
+        while (opModeIsActive()){
+            double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
+            double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1};
+            double [] y = new double[lm_p.length];
+            double [] x = new double[lm_p.length];
+            
+            for(int i = 0; i< lm_p.length; i++){
+                while (opModeIsActive() && Yaw < 90){
+                    lm.setPower(lm_p[i]);
+                    rm.setPower(rm_p[i]);
+                    robotOrientation = imu.getRobotYawPitchRollAngles();
+                    Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                    telemetry.addData("Yaw ", Yaw);
+                    telemetry.addData("I", i);
+                    telemetry.update();
+                    }
+                
+                lm.setPower(0);
+                rm.setPower(0);
+                
+                robotOrientation = imu.getRobotYawPitchRollAngles();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                
+                x [i]= Yaw - 90.0;
+                
+                imu.resetYaw();
+                robotOrientation = imu.getRobotYawPitchRollAngles();
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                telemetry.addData("Yaw", Yaw);
+                telemetry.update();
+                /*
+                Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
+                telemetry.addData("Yaw", Yaw);
+                telemetry.update();*/
+            }
+            while (opModeIsActive()){
+                telemetry.addData("0.1", x[0]);
+                telemetry.addData("0.2", x[1]);
+                telemetry.addData("0.3", x[2]);
+                telemetry.addData("0.4", x[3]);
+                telemetry.addData("0.5", x[4]);
+                telemetry.addData("0.6", x[5]);
+                telemetry.addData("0.7", x[6]);
+                telemetry.addData("0.8", x[7]);
+                telemetry.addData("0.9", x[8]);
+                telemetry.addData("1", x[9]);
+                telemetry.update();
+            }
+        }
+    }
+}