werobot/ftc2024-robotcode
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

helloworldhellow

Browse source
This commit is contained in:
GZod01 2024-03-08 14:30:01 +01:00
commit f1b4a60c76
5 changed files with 462 additions and 39 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

184
IMU.java Normal file
View file

@ -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;
}
}

111
autonome_api.java Normal file
View file

@ -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);
}
}

94
ftc2024-autonome.java
View file

@ -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.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
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 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");
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
}
}
}

10
helloworld.py.java
View file

@ -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);
""";
```
*/

102
test.java Normal file
View file

@ -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();
}
}
}
}