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

updates

Browse source
This commit is contained in:
Zelina974 2024-02-11 16:17:58 +01:00
parent 6f7b315ea6
commit 08cdb2a091
1 changed files with 100 additions and 94 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

188
ftc2024-autonome.java
View file

@ -14,6 +14,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.IMU;
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;
@ -27,108 +28,113 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
@Autonomous @Autonomous
public class ftc2024_autonome extends LinearOpMode { public class ftc2024_autonome extends LinearOpMode {
private DcMotor rm; private DcMotor rm;
private DcMotor lm; private DcMotor lm;
private IMU imu; private IMU imu;
private ElapsedTime runtime = new ElapsedTime(); private ElapsedTime runtime = new ElapsedTime();
public double time_for_dist(double speed, double dist){ public double time_for_dist(double speed, double dist){
return (double) (dist/speed); return (double) (dist/speed);
} }
@Override @Override
public void runOpMode() { 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");
imu = hardwareMap.get(IMU.class, "imu"); imu = hardwareMap.get(IMU.class, "imu");
imu.initialize( imu.initialize(
new IMU.Parameters( new IMU.Parameters(
new RevHubOrientationOnRobot( new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
) )
) )
); );
imu.resetYaw(); imu.resetYaw();
rm.setDirection(DcMotorSimple.Direction.REVERSE); rm.setDirection(DcMotorSimple.Direction.REVERSE);
telemetry.addData("Status", "Initialized"); telemetry.addData("Status", "Initialized");
telemetry.update(); telemetry.update();
double tour_par_minute = 300.0; double tour_par_minute = 300.0;
double wheel_width = 9.0e-2; double wheel_width = 9.0e-2;
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 = true; boolean mode = true;
// Wait for the game to start (driver presses PLAY)
waitForStart();
runtime.reset(); YawPitchRollAngles robotOrientation;
if (mode){ robotOrientation = imu.getRobotYawPitchRollAngles();
//mode Elina
while (opModeIsActive() && (runtime.seconds() <= Yaw != 90)) { double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
lm.setPower(1); double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
rm.setPower(-1); double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
telemetry.addData("Leg 1", runtime.seconds());
telemetry.update(); // Wait for the game to start (driver presses PLAY)
} waitForStart();
runtime.reset();
while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { runtime.reset();
lm.setPower(1); if (mode){
rm.setPower(1); //mode Elina
telemetry.addData("Leg 2", runtime.seconds()); while (opModeIsActive() && Yaw <= 90.0) {
telemetry.update(); lm.setPower(0.5);
} rm.setPower(-0.5);
} Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
else { telemetry.addData("Leg 1", runtime.seconds());
double[][] operations = { telemetry.update();
{-1.0,1.0}, // vectors }
{1.0,1.0}, runtime.reset();
{-1.0,1.0}, while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
{-1.0,-1.0}, lm.setPower(1);
{1.0,-1.0} rm.setPower(1);
}; telemetry.addData("Leg 2", runtime.seconds());
//mode Aurelien telemetry.update();
for(int i = 0; i<operations.length; i++){ }
double[] vec = operations[i]; }
double x = vec[0]; else {
double y = vec[1]; double[][] operations = {
double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2)); {-1.0,1.0}, // vectors
double time = time_for_dist(speed, total_dist); {1.0,1.0},
double a = (-y+x)/Math.pow(2,1/2); {-1.0,1.0},
double b = (-y-x)/Math.pow(2,1/2); {-1.0,-1.0},
double vmean = (Math.abs(a)+Math.abs(b))/2; {1.0,-1.0}
double lmvalue = (a/vmean); };
double rmvalue = (b/vmean); //mode Aurelien
runtime.reset(); for(int i = 0; i<operations.length; i++){
while (opModeIsActive() && (runtime.seconds() <= time)) { double[] vec = operations[i];
lm.setPower(lmvalue); double x = vec[0];
rm.setPower(rmvalue); double y = vec[1];
telemetry.addData("Runtime Seconds", runtime.seconds()); double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
telemetry.addData("current_operation",operations[i]); double time = time_for_dist(speed, total_dist);
telemetry.addData("current_op_id",i); 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();
} }
} }
} }
YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles();
// Now use these simple methods to extract each angle // Now use these simple methods to extract each angle
// (Java type double) from the object you just created: // (Java type double) from the object you just created:
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
telemetry.addData("yaw",Yaw);
telemetry.update(); telemetry.addData("yaw",Yaw);
// run until the end of the match (driver presses STOP
} telemetry.update();
// run until the end of the match (driver presses STOP
}
} }