diff --git a/IMU.java b/IMU.java new file mode 100644 index 0000000..6d21439 --- /dev/null +++ b/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; + } +} + diff --git a/autonome_api.java b/autonome_api.java new file mode 100644 index 0000000..7437fda --- /dev/null +++ b/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)>>>>>> 7cd7be1f14eec5f1be12b9f60543771de58ca782 @Override + public void runOpMode() { - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.class, "brm"); + 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( @@ -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 Roll = robotOrientation.getRoll(AngleUnit.DEGREES); + double yaw_sortie = 0.0; - 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 - } - -} - +} \ No newline at end of file diff --git a/helloworld.py.java b/helloworld.py.java index ad926e0..f20944e 100644 --- a/helloworld.py.java +++ b/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); -"""; +``` +*/ diff --git a/test.java b/test.java new file mode 100644 index 0000000..3b9b3f8 --- /dev/null +++ b/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(); + } + } + } +} \ No newline at end of file