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 15:48:03 +01:00
parent 079817e180
commit a9129baf8d
1 changed files with 108 additions and 72 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

38
ftc2024-autonome.java
View file

@ -13,12 +13,23 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple; 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.ImuOrientationOnRobot;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
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;
@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 ElapsedTime runtime = new ElapsedTime(); private ElapsedTime runtime = new ElapsedTime();
@ -30,6 +41,18 @@ public class ftc2024_autonome extends LinearOpMode {
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.initialize(
new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)
)
);
imu.resetYaw();
rm.setDirection(DcMotorSimple.Direction.REVERSE); rm.setDirection(DcMotorSimple.Direction.REVERSE);
telemetry.addData("Status", "Initialized"); telemetry.addData("Status", "Initialized");
telemetry.update(); telemetry.update();
@ -45,7 +68,7 @@ public class ftc2024_autonome extends LinearOpMode {
runtime.reset(); runtime.reset();
if (mode){ if (mode){
//mode Elina //mode Elina
while (opModeIsActive() && (runtime.seconds() <= 41e-2*Math.PI/4/speed)) { while (opModeIsActive() && (runtime.seconds() <= Yaw != 90)) {
lm.setPower(1); lm.setPower(1);
rm.setPower(-1); rm.setPower(-1);
telemetry.addData("Leg 1", runtime.seconds()); telemetry.addData("Leg 1", runtime.seconds());
@ -86,10 +109,23 @@ public class ftc2024_autonome extends LinearOpMode {
telemetry.addData("Runtime Seconds", runtime.seconds()); telemetry.addData("Runtime Seconds", runtime.seconds());
telemetry.addData("current_operation",operations[i]); telemetry.addData("current_operation",operations[i]);
telemetry.addData("current_op_id",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
// (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();
// run until the end of the match (driver presses STOP // run until the end of the match (driver presses STOP
} }