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

change

Browse source
This commit is contained in:
Zelina974 2024-02-26 14:10:42 +01:00
parent abe8cb95fa
commit 94118433a0
2 changed files with 145 additions and 55 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

130
ftc2024-autonome.java
View file

@ -17,7 +17,6 @@ import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot; import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
@ -63,7 +62,7 @@ public class ftc2024_autonome extends LinearOpMode {
double wheel_rayon = (wheel_width)/2; double wheel_rayon = (wheel_width)/2;
double wheel_perimeter = wheel_rayon*2*Math.PI; double wheel_perimeter = wheel_rayon*2*Math.PI;
double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second
boolean mode = false; boolean mode = true;
YawPitchRollAngles robotOrientation; YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles(); robotOrientation = imu.getRobotYawPitchRollAngles();
@ -72,7 +71,7 @@ public class ftc2024_autonome extends LinearOpMode {
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES); double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES); double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
double yaw_sortie; double yaw_sortie;
// Wait for the game to start (driver presses PLAY) // Wait for the game to start (driver presses PLAY)
waitForStart(); waitForStart();
@ -80,69 +79,92 @@ public class ftc2024_autonome extends LinearOpMode {
runtime.reset(); runtime.reset();
if (mode){ if (mode){
//mode Elina //mode Elina
while (opModeIsActive() && Yaw <= 90.0) { /*while (opModeIsActive() && Yaw <= -90.0) {
lm.setPower(0.5); lm.setPower(0.2);
rm.setPower(-0.5); rm.setPower(-0.2);
robotOrientation = imu.getRobotYawPitchRollAngles(); robotOrientation = imu.getRobotYawPitchRollAngles();
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
telemetry.addData("Yaw : ", Yaw); telemetry.addData("Yaw : ", Yaw);
telemetry.update(); telemetry.update();
yaw_sortie = Yaw;
}*/
while (opModeIsActive() && Yaw < 90) {
lm.setPower(0.2);
rm.setPower(-0.2);
robotOrientation = imu.getRobotYawPitchRollAngles();
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
telemetry.addData("Leg 1", runtime.seconds());
telemetry.addData("Yaw", Yaw);
telemetry.update();
} }
telemetry.addData("yaw_sortie", yaw_sortie);
runtime.reset(); runtime.reset();
while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { yaw_sortie = Yaw;
telemetry.update();
while (opModeIsActive()) {
lm.setPower(0.1);
rm.setPower(0.1);
robotOrientation = imu.getRobotYawPitchRollAngles();
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
telemetry.addData("yaw_sortie", yaw_sortie);
telemetry.addData("Yaw", Yaw);
telemetry.update();
}
/*while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
lm.setPower(0.1); lm.setPower(0.1);
rm.setPower(0.1); rm.setPower(0.1);
telemetry.addData("Leg 2", runtime.seconds()); telemetry.addData("Leg 2", runtime.seconds());
telemetry.update(); telemetry.update();
} }*/
} }
else{ else{
while(opModeIsActive()){ while(opModeIsActive()){
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
if(Math.abs(Yaw-90.0)<=0.01){ if(Math.abs(Yaw-90.0)<=0.01){
break; break;
} }
else if((Yaw - 90.0) <0){ else if((Yaw - 90.0) <0){
lm.setPower((Math.abs(Yaw-90.0)/90)*0.5); lm.setPower((Math.abs(Yaw-90.0)/90)*0.5);
rm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); rm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5);
} }
else{ else{
rm.setPower((Math.abs(Yaw-90.0)/90)*0.5); rm.setPower((Math.abs(Yaw-90.0)/90)*0.5);
lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5);
}
} }
if(false){
double[][] operations = {
{-1.0,1.0}, // vectors
{1.0,1.0},
{-1.0,1.0},
{-1.0,-1.0},
{1.0,-1.0}
};
//mode Aurelien
for(int i = 0; i<operations.length; i++){
double[] vec = operations[i];
double x = vec[0];
double y = vec[1];
double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
double time = time_for_dist(speed, total_dist);
double a = (-y+x)/Math.pow(2,1/2);
double b = (-y-x)/Math.pow(2,1/2);
double vmean = (Math.abs(a)+Math.abs(b))/2;
double lmvalue = (a/vmean);
double rmvalue = (b/vmean);
runtime.reset();
while (opModeIsActive() && (runtime.seconds() <= time)) { if(false){
lm.setPower(lmvalue); double[][] operations = {
rm.setPower(rmvalue); {-1.0,1.0}, // vectors
telemetry.addData("Runtime Seconds", runtime.seconds()); {1.0,1.0},
telemetry.addData("current_operation",operations[i]); {-1.0,1.0},
telemetry.addData("current_op_id",i); {-1.0,-1.0},
{1.0,-1.0}
};
//mode Aurelien
for(int i = 0; i<operations.length; i++){
double[] vec = operations[i];
double x = vec[0];
double y = vec[1];
double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
double time = time_for_dist(speed, total_dist);
double a = (-y+x)/Math.pow(2,1/2);
double b = (-y-x)/Math.pow(2,1/2);
double vmean = (Math.abs(a)+Math.abs(b))/2;
double lmvalue = (a/vmean);
double rmvalue = (b/vmean);
runtime.reset();
while (opModeIsActive() && (runtime.seconds() <= time)) {
lm.setPower(lmvalue);
rm.setPower(rmvalue);
telemetry.addData("Runtime Seconds", runtime.seconds());
telemetry.addData("current_operation",operations[i]);
telemetry.addData("current_op_id",i);
telemetry.update();
telemetry.update();
}
} }
} }
} }
@ -154,9 +176,9 @@ public class ftc2024_autonome extends LinearOpMode {
telemetry.update(); telemetry.update();
// run until the end of the match (driver presses STOP // run until the end of the match (driver presses STOP
} }
} }
} }

68
test.java
View file

@ -1 +1,69 @@
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, "lm");
rm = hardwareMap.get (DcMotor.class, "rm");
rm.setDirection(DcMotorSimple.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;
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double yaw_sortie1;
double yaw_sortie2;
double yaw_sortie3;
double yaw_sortie4;
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};
for(int i = 0; i< p_t_g.length; i++){
while (opModeIsActive() && Yaw < 90){
lm.setPower = lm_p[i];
rm.setPower = rm_p[i];
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
telemetry.addData("Yaw : ", Yaw);
telemetry.update();
}
}
}
}
}