fix
This commit is contained in:
Zelina974
parent
da49742d7c
commit
3f523e5e12
1 changed files with 99 additions and 143 deletions
|
|
@ -1,47 +1,36 @@
|
||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
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 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.Orientation;
|
||||||
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;
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
|
||||||
|
|
||||||
|
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
|
@Autonomous
|
||||||
|
|
||||||
public class ftc2024_autonome extends LinearOpMode {
|
public class ftc2024_autonome_test 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();
|
|
||||||
|
|
||||||
|
|
||||||
public double time_for_dist(double speed, double dist){
|
|
||||||
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");
|
||||||
|
|
||||||
|
rm.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
|
||||||
imu = hardwareMap.get(IMU.class, "imu");
|
imu = hardwareMap.get(IMU.class, "imu");
|
||||||
imu.initialize(
|
imu.initialize(
|
||||||
|
|
@ -53,132 +42,99 @@ public class ftc2024_autonome extends LinearOpMode {
|
||||||
)
|
)
|
||||||
);
|
);
|
||||||
imu.resetYaw();
|
imu.resetYaw();
|
||||||
|
|
||||||
rm.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
||||||
telemetry.addData("Status", "Initialized");
|
|
||||||
telemetry.update();
|
|
||||||
double tour_par_minute = 300.0;
|
|
||||||
double wheel_width = 9.0e-2;
|
|
||||||
double wheel_rayon = (wheel_width)/2;
|
|
||||||
double wheel_perimeter = wheel_rayon*2*Math.PI;
|
|
||||||
double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second
|
|
||||||
boolean mode = true;
|
|
||||||
|
|
||||||
YawPitchRollAngles robotOrientation;
|
YawPitchRollAngles robotOrientation;
|
||||||
robotOrientation = imu.getRobotYawPitchRollAngles();
|
robotOrientation = imu.getRobotYawPitchRollAngles();
|
||||||
|
|
||||||
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
|
double yaw_sortie = 0.0;
|
||||||
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
|
|
||||||
|
|
||||||
double yaw_sortie;
|
|
||||||
|
|
||||||
// Wait for the game to start (driver presses PLAY)
|
|
||||||
waitForStart();
|
waitForStart();
|
||||||
|
|
||||||
runtime.reset();
|
while (opModeIsActive()){
|
||||||
if (mode){
|
double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
|
||||||
//mode Elina
|
double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1};
|
||||||
/*while (opModeIsActive() && Yaw <= -90.0) {
|
double [] x = new double[lm_p.length];
|
||||||
lm.setPower(0.2);
|
for(int i = 0; i< lm_p.length; i++){
|
||||||
rm.setPower(-0.2);
|
while (opModeIsActive() && Yaw < 90){
|
||||||
robotOrientation = imu.getRobotYawPitchRollAngles();
|
lm.setPower(lm_p[i]);
|
||||||
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
rm.setPower(rm_p[i]);
|
||||||
telemetry.addData("Yaw : ", Yaw);
|
robotOrientation = imu.getRobotYawPitchRollAngles();
|
||||||
telemetry.update();
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
telemetry.addData("Yaw : ", Yaw);
|
||||||
}*/
|
telemetry.update();
|
||||||
while (opModeIsActive() && Yaw < 90) {
|
yaw_sortie = Yaw;
|
||||||
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();
|
|
||||||
}
|
|
||||||
runtime.reset();
|
|
||||||
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);
|
|
||||||
rm.setPower(0.1);
|
|
||||||
telemetry.addData("Leg 2", runtime.seconds());
|
|
||||||
telemetry.update();
|
|
||||||
}*/
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
while(opModeIsActive()){
|
|
||||||
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
|
||||||
if(Math.abs(Yaw-90.0)<=0.01){
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
else if((Yaw - 90.0) <0){
|
|
||||||
lm.setPower((Math.abs(Yaw-90.0)/90)*0.5);
|
|
||||||
rm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
rm.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)) {
|
|
||||||
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.addData("Yaw sortie", yaw_sortie);
|
||||||
|
telemetry.update();
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
x [i]= Yaw - yaw_sortie;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*if (i = 0) {
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
a = yaw_sortie;
|
||||||
|
a1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 1){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
b = yaw_sortie;
|
||||||
|
b1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 2){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double c = yaw_sortie;
|
||||||
|
double c1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 3){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double d = yaw_sortie;
|
||||||
|
double d1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 4){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double e = yaw_sortie;
|
||||||
|
double e1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 5){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double f = yaw_sortie;
|
||||||
|
double f1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 6){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double g = yaw_sortie;
|
||||||
|
double g1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 7){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double h = yaw_sortie;
|
||||||
|
double h1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 8){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
double j = yaw_sortie;
|
||||||
|
double j1 = Yaw;
|
||||||
|
}
|
||||||
|
if (i = 9){
|
||||||
|
Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
|
k = yaw_sortie;
|
||||||
|
k1 = Yaw;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
imu.resetYaw();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
Loading…
Reference in a new issue