ftc2024-robotcode/ftc2024-carlike.java

82 lines
3.3 KiB
Java
Raw Normal View History

2023-12-17 17:10:23 +00:00
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.robot.Robot;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.robot.Robot;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot")
public class Werobot_FTC2024_carlike extends LinearOpMode {
private DcMotor rm;
private DcMotor lm;
@Override
public void runOpMode() throws InterruptedException {
float x;
double y;
2024-01-06 14:52:12 +00:00
double t;
2023-12-17 17:10:23 +00:00
String mode = "normal";
boolean already_a = false;
telemetry.addData("Status", "Initialized");
telemetry.update();
lm = hardwareMap.get(DcMotor.class, "blm");
rm = hardwareMap.get(DcMotor.class, "brm");
waitForStart();
while (opModeIsActive()) {
2024-01-06 14:52:12 +00:00
x = gamepad1.left_stick_x;
y = gamepad1.left_stick_y;
t= gamepad1.left_trigger;
2023-12-17 17:10:23 +00:00
telemetry.addData("Status", "Running");
if(gamepad1.a && !already_a){
if(mode=="normal"){
mode="tank";
2024-01-06 13:55:04 +00:00
}else if(mode=="tank"){
mode = "essaifranck";
}else{
2023-12-17 17:10:23 +00:00
mode="normal";
}
already_a = true;
}
if(!gamepad1.a && already_a){
already_a = false;
}
double lpower = 0.0;
double rpower = 0.0;
if(mode=="normal"){
2024-01-06 14:52:12 +00:00
double ysign = y>0?1.0:(y<0?-1.0:0.0);
double xsign = x>0?1.0:(x<0?-1.0:0.0);
lpower = -ysign * t + (xsign-2*x)*t;
rpower = ysign * t + (xsign-2*x)*t;
2023-12-17 17:10:23 +00:00
}
else if (mode=="tank"){
2024-01-06 14:52:12 +00:00
lpower = -y;
2023-12-17 17:10:23 +00:00
rpower = gamepad1.right_stick_y;
}
2024-01-06 13:55:04 +00:00
else if (mode=="essaifranck"){
2024-01-06 14:52:12 +00:00
double a = (x-y)/Math.pow(2,1/2);
double b = (x+y)/Math.pow(2,1/2);
double vmean = (a+b)/2;
lpower = (a/vmean)*t;
rpower = (b/vmean)*t;
2024-01-06 13:55:04 +00:00
}
2023-12-17 17:10:23 +00:00
if(!(gamepad1.left_bumper)){
lpower/=3;
rpower/=3;
}
lm.setPower(lpower);
rm.setPower(rpower);
2024-01-06 14:52:12 +00:00
telemetry.addData("ltrigg",t);
2023-12-17 17:10:23 +00:00
telemetry.addData("mode",mode);
telemetry.update();
}
}
}