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

format tabulations in ftc2024-carlike.java, add imu

Browse source
This commit is contained in:
GZod01 2024-01-07 15:29:01 +01:00
parent b770f1a271
commit a89a69841e
1 changed files with 91 additions and 88 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

179
ftc2024-carlike.java
View file

@ -14,91 +14,94 @@ import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot") @TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot")
public class Werobot_FTC2024_carlike extends LinearOpMode { public class Werobot_FTC2024_carlike extends LinearOpMode {
private DcMotor rm; private DcMotor rm;
private DcMotor lm; private DcMotor lm;
private double helloexp(double t){ private IntegratingGyroscope imu;
return (Math.exp(5*t)-1)/(Math.exp(5)-1); private double helloexp(double t){
} return (Math.exp(5*t)-1)/(Math.exp(5)-1);
@Override }
public void runOpMode() throws InterruptedException { @Override
float x; public void runOpMode() throws InterruptedException {
double y; float x;
double t; double y;
double t2; double t;
double t3; double t2;
String mode = "normal"; double t3;
boolean already_a = false; String mode = "normal";
telemetry.addData("Status", "Initialized"); boolean already_a = false;
telemetry.update(); telemetry.addData("Status", "Initialized");
lm = hardwareMap.get(DcMotor.class, "blm"); telemetry.update();
rm = hardwareMap.get(DcMotor.class, "brm"); lm = hardwareMap.get(DcMotor.class, "blm");
rm.setDirection(DcMotorSimple.Direction.REVERSE); rm = hardwareMap.get(DcMotor.class, "brm");
waitForStart(); imu = hardwareMap.get(IntegratingGyroscope.class, "imu");
rm.setDirection(DcMotorSimple.Direction.REVERSE);
waitForStart();
while (opModeIsActive()) {
x = gamepad1.left_stick_x;
y = gamepad1.left_stick_y; while (opModeIsActive()) {
t= gamepad1.right_trigger; x = gamepad1.left_stick_x;
t2 = helloexp(t); y = gamepad1.left_stick_y;
t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2))); t= gamepad1.right_trigger;
telemetry.addData("Status", "Running"); t2 = helloexp(t);
if(gamepad1.a && !already_a){ t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2)));
if(mode=="normal"){ telemetry.addData("Status", "Running");
mode="tank"; if(gamepad1.a && !already_a){
}else if(mode=="tank"){ if(mode=="normal"){
mode = "essaifranck"; mode="tank";
}else if (mode == "essaifranck"){ }else if(mode=="tank"){
mode = "elina"; mode = "essaifranck";
}else{ }else if (mode == "essaifranck"){
mode="normal"; mode = "elina";
} }else{
already_a = true; mode="normal";
} }
if(!gamepad1.a && already_a){ already_a = true;
already_a = false; }
} if(!gamepad1.a && already_a){
double lpower = 0.0; already_a = false;
double rpower = 0.0; }
if(mode=="normal"){ double lpower = 0.0;
double ysign = y>0?1.0:(y<0?-1.0:0.0); double rpower = 0.0;
double xsign = x>0?1.0:(x<0?-1.0:0.0); if(mode=="normal"){
lpower = -ysign * t + (xsign-2*x)*t; double ysign = y>0?1.0:(y<0?-1.0:0.0);
rpower = ysign * t + (xsign-2*x)*t; double xsign = x>0?1.0:(x<0?-1.0:0.0);
} lpower = -ysign * t + (xsign-2*x)*t;
else if (mode=="tank"){ rpower = ysign * t + (xsign-2*x)*t;
lpower = -y; }
rpower = gamepad1.right_stick_y; else if (mode=="tank"){
} lpower = -y;
else if (mode=="essaifranck"){ rpower = gamepad1.right_stick_y;
double a = (-y+x)/Math.pow(2,1/2); }
double b = (-y-x)/Math.pow(2,1/2); else if (mode=="essaifranck"){
double vmean = (Math.abs(a)+Math.abs(b))/2; double a = (-y+x)/Math.pow(2,1/2);
lpower = (a/vmean)*t2; double b = (-y-x)/Math.pow(2,1/2);
rpower = (b/vmean)*t2; double vmean = (Math.abs(a)+Math.abs(b))/2;
} lpower = (a/vmean)*t2;
else if (mode=="elina"){ rpower = (b/vmean)*t2;
double a = (-y+x)/Math.pow(2,1/2); }
double b = (-y-x)/Math.pow(2,1/2); else if (mode=="elina"){
double vmean = (Math.abs(a)+Math.abs(b))/2; double a = (-y+x)/Math.pow(2,1/2);
lpower = (a/vmean)*t3; double b = (-y-x)/Math.pow(2,1/2);
rpower = (b/vmean)*t3; double vmean = (Math.abs(a)+Math.abs(b))/2;
} lpower = (a/vmean)*t3;
if(!(gamepad1.left_bumper)){ rpower = (b/vmean)*t3;
lpower/=3; }
rpower/=3; if(!(gamepad1.left_bumper)){
} lpower/=3;
lm.setPower(lpower); rpower/=3;
rm.setPower(rpower); }
lm.setPower(lpower);
telemetry.addData("x",x); rm.setPower(rpower);
telemetry.addData("y",y);
telemetry.addData("lpow",lpower); telemetry.addData("x",x);
telemetry.addData("rpow",rpower); telemetry.addData("y",y);
telemetry.addData("ltrigg",t); telemetry.addData("lpow",lpower);
telemetry.addData("t2",t2); telemetry.addData("rpow",rpower);
telemetry.addData("mode",mode); telemetry.addData("ltrigg",t);
telemetry.update(); telemetry.addData("t2",t2);
} telemetry.addData("mode",mode);
} telemetry.addData("imu angular orientation", imu.getAngularOrientation());
} telemetry.update();
}
}
}