hello
This commit is contained in:
parent
6f7b315ea6
commit
d51fb81ef7
1 changed files with 252 additions and 138 deletions
|
|
@ -25,155 +25,269 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||||
//test
|
//test
|
||||||
@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 {
|
||||||
|
/*
|
||||||
|
* Le moteur de droite
|
||||||
|
*/
|
||||||
private DcMotor rm;
|
private DcMotor rm;
|
||||||
|
/*
|
||||||
|
* Le moteur de gauche
|
||||||
|
*/
|
||||||
private DcMotor lm;
|
private DcMotor lm;
|
||||||
|
/*
|
||||||
|
* La moissoneuse
|
||||||
|
*/
|
||||||
private DcMotor moissoneuse;
|
private DcMotor moissoneuse;
|
||||||
|
/*
|
||||||
|
* L'IMU
|
||||||
|
*/
|
||||||
private IMU imu;
|
private IMU imu;
|
||||||
|
/*
|
||||||
|
* La fonction pour faire des exponentielles spécifiques
|
||||||
|
* @param double t => le nombre dont on veut faire l'exponentielle
|
||||||
|
* @return double une_exponentielle_très_spéciale_de_t
|
||||||
|
*/
|
||||||
private double helloexp(double t){
|
private double helloexp(double t){
|
||||||
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
|
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
|
||||||
}
|
}
|
||||||
|
/*
|
||||||
|
* La fonction du thread principal
|
||||||
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() throws InterruptedException {
|
public void runOpMode() throws InterruptedException {
|
||||||
float x;
|
/*
|
||||||
double y;
|
* l'axe x du joystick gauche de la manette
|
||||||
double t;
|
*/
|
||||||
double t2;
|
float x;
|
||||||
double t3;
|
/*
|
||||||
String mode = "normal";
|
* l'axe y du joystick gauche de la manette
|
||||||
boolean already_b = false;
|
*/
|
||||||
boolean already_a = false;
|
double y;
|
||||||
boolean already_x = false;
|
/*
|
||||||
telemetry.addData("Status", "Initialized");
|
* variation 1 du left trigger
|
||||||
telemetry.update();
|
*/
|
||||||
lm = hardwareMap.get(DcMotor.class, "blm");
|
double t;
|
||||||
rm = hardwareMap.get(DcMotor.class, "brm");
|
/*
|
||||||
moissoneuse = hardwareMap.get(DcMotor.class, "flm");
|
* variation 2 du left trigger
|
||||||
|
*/
|
||||||
|
double t2;
|
||||||
|
/*
|
||||||
|
* variation 3 du left trigger
|
||||||
|
*/
|
||||||
|
double t3;
|
||||||
|
/*
|
||||||
|
* le mode du robot
|
||||||
|
*/
|
||||||
|
String mode = "normal";
|
||||||
|
/*
|
||||||
|
* b est il déjà préssé?
|
||||||
|
*/
|
||||||
|
boolean already_b = false;
|
||||||
|
/*
|
||||||
|
* a est il déjà préssé?
|
||||||
|
*/
|
||||||
|
boolean already_a = false;
|
||||||
|
/*
|
||||||
|
* x est il déjà préssé?
|
||||||
|
*/
|
||||||
|
boolean already_x = false;
|
||||||
|
/*
|
||||||
|
* ajout de la donnée status sur telemetry, initialisé
|
||||||
|
*/
|
||||||
|
telemetry.addData("Status", "Initialized");
|
||||||
|
/*
|
||||||
|
* mise a jour de la telemetry
|
||||||
|
*/
|
||||||
|
telemetry.update();
|
||||||
|
/*
|
||||||
|
* récupération du moteur gauche pour {@link lm}
|
||||||
|
*/
|
||||||
|
lm = hardwareMap.get(DcMotor.class, "blm");
|
||||||
|
/*
|
||||||
|
* récupération du moteur droit pour {@link rm}
|
||||||
|
*/
|
||||||
|
rm = hardwareMap.get(DcMotor.class, "brm");
|
||||||
|
/*
|
||||||
|
* récupération du moteur de moissoneuse pour {@link moissoneuse}
|
||||||
|
*/
|
||||||
|
moissoneuse = hardwareMap.get(DcMotor.class, "flm");
|
||||||
|
|
||||||
|
/*
|
||||||
|
* récupération de l'imu pour {@link imu}
|
||||||
|
*/
|
||||||
|
imu = hardwareMap.get(IMU.class, "imu");
|
||||||
|
/*
|
||||||
|
* initialisation de l'imu
|
||||||
|
*/
|
||||||
|
imu.initialize(
|
||||||
|
/*
|
||||||
|
* paramètres de l'imu
|
||||||
|
*/
|
||||||
|
new IMU.Parameters(
|
||||||
|
/*
|
||||||
|
* orientation initiale du robot
|
||||||
|
*/
|
||||||
|
new RevHubOrientationOnRobot(
|
||||||
|
/*
|
||||||
|
* logo vers le haut
|
||||||
|
*/
|
||||||
|
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
||||||
|
/*
|
||||||
|
* usb vers l'avant
|
||||||
|
*/
|
||||||
|
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
|
||||||
|
)
|
||||||
|
)
|
||||||
|
);
|
||||||
|
/*
|
||||||
|
* réinitialisation du yaw de l'imu
|
||||||
|
*/
|
||||||
|
imu.resetYaw();
|
||||||
|
//telemetry.addData("Mode", "calibrating...");
|
||||||
|
//telemetry.update();
|
||||||
|
|
||||||
|
// make sure the imu gyro is calibrated before continuing.
|
||||||
|
//while (!isStopRequested() && !imu.isGyroCalibrated())
|
||||||
|
//{
|
||||||
|
// sleep(50);
|
||||||
|
// idle();
|
||||||
|
//}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* ajout de la donnée en "mode": "en attente de démarrage" sur telemetry
|
||||||
|
*/
|
||||||
|
telemetry.addData("Mode", "waiting for start");
|
||||||
|
//telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
|
||||||
|
/*
|
||||||
|
* mise à jour de la telemetry
|
||||||
|
*/
|
||||||
|
telemetry.update();
|
||||||
|
/*
|
||||||
|
* en attente du démarrage
|
||||||
|
*/
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
/*
|
||||||
|
* le robot a démarré, le tant que le robot est activé et donc qu'il n'a pas été stoppé:
|
||||||
|
*/
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
/*
|
||||||
|
* définition de {@link x} sur la valeur de x du joystick gauche du gamepad 1
|
||||||
|
*/
|
||||||
|
x = gamepad1.left_stick_x;
|
||||||
|
/*
|
||||||
|
* définition de {@link y} sur la valeur de y du joystick gauche du gamepad 1
|
||||||
|
*/
|
||||||
|
y = gamepad1.left_stick_y;
|
||||||
|
/*
|
||||||
|
* définition de {@link t} sur la valeur du trigger droit du gamepad 1
|
||||||
|
*/
|
||||||
|
t= gamepad1.right_trigger;
|
||||||
|
/*
|
||||||
|
* définition de {@link t2} par utilisation de la fonction {@link helloexp} sur {@link t}
|
||||||
|
*/
|
||||||
|
t2 = helloexp(t);
|
||||||
|
/*
|
||||||
|
* définition de {@link t3} par utilisation de la fonction {@link helloexp} sur la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link x} au carré plus {@link y} au carré
|
||||||
|
*/
|
||||||
|
t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2)));
|
||||||
|
/*
|
||||||
|
* ajout de la donnée "statut":"entrain de courrir" sur telemetry
|
||||||
|
*/
|
||||||
|
telemetry.addData("Status", "Running");
|
||||||
|
/*
|
||||||
|
* si le bouton a du gamepad 1 est appuyé et {already_a} est faux
|
||||||
|
*/
|
||||||
|
if(gamepad1.a && !already_a){
|
||||||
|
if(mode=="normal"){
|
||||||
|
mode="tank";
|
||||||
|
}else if(mode=="tank"){
|
||||||
|
mode = "essaifranck";
|
||||||
|
}else if (mode == "essaifranck"){
|
||||||
|
mode = "elina";
|
||||||
|
}else{
|
||||||
|
mode="normal";
|
||||||
|
}
|
||||||
|
already_a = true;
|
||||||
|
}
|
||||||
|
if(!gamepad1.a && already_a){
|
||||||
|
already_a = false;
|
||||||
|
}
|
||||||
|
double lpower = 0.0;
|
||||||
|
double rpower = 0.0;
|
||||||
|
if(mode=="normal"){
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
else if (mode=="tank"){
|
||||||
|
lpower = -y;
|
||||||
|
rpower = gamepad1.right_stick_y;
|
||||||
|
}
|
||||||
|
else if (mode=="essaifranck"){
|
||||||
|
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;
|
||||||
|
lpower = (a/vmean)*t2;
|
||||||
|
rpower = (b/vmean)*t2;
|
||||||
|
}
|
||||||
|
else if (mode=="elina"){
|
||||||
|
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;
|
||||||
|
lpower = (a/vmean)*t3;
|
||||||
|
rpower = (b/vmean)*t3;
|
||||||
|
}
|
||||||
|
if(!(gamepad1.left_bumper)){
|
||||||
|
lpower/=3;
|
||||||
|
rpower/=3;
|
||||||
|
}
|
||||||
|
lm.setPower(lpower);
|
||||||
|
rm.setPower(rpower);
|
||||||
|
|
||||||
|
if(gamepad1.b && !already_b){
|
||||||
|
already_b = !already_b;
|
||||||
|
if(moissoneuse.getPower() == 1.0){
|
||||||
|
moissoneuse.setPower(0);
|
||||||
|
}else{
|
||||||
|
moissoneuse.setPower(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(!gamepad1.b && already_b){
|
||||||
|
already_b = !already_b;
|
||||||
|
}
|
||||||
|
if(gamepad1.x && !already_x){
|
||||||
|
already_x = !already_x;
|
||||||
|
if(moissoneuse.getPower() == -1.0){
|
||||||
|
moissoneuse.setPower(0);
|
||||||
|
}else{
|
||||||
|
moissoneuse.setPower(-1);
|
||||||
|
}
|
||||||
|
if(!gamepad1.x && already_x){
|
||||||
|
already_x = !already_x;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
imu = hardwareMap.get(IMU.class, "imu");
|
telemetry.addData("x",x);
|
||||||
imu.initialize(
|
telemetry.addData("y",y);
|
||||||
new IMU.Parameters(
|
telemetry.addData("lpow",lpower);
|
||||||
new RevHubOrientationOnRobot(
|
telemetry.addData("rpow",rpower);
|
||||||
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
telemetry.addData("ltrigg",t);
|
||||||
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
|
telemetry.addData("t2",t2);
|
||||||
)
|
telemetry.addData("mode",mode);
|
||||||
)
|
// Create an object to receive the IMU angles
|
||||||
);
|
YawPitchRollAngles robotOrientation;
|
||||||
imu.resetYaw();
|
robotOrientation = imu.getRobotYawPitchRollAngles();
|
||||||
//telemetry.addData("Mode", "calibrating...");
|
|
||||||
//telemetry.update();
|
|
||||||
|
|
||||||
// make sure the imu gyro is calibrated before continuing.
|
// Now use these simple methods to extract each angle
|
||||||
//while (!isStopRequested() && !imu.isGyroCalibrated())
|
// (Java type double) from the object you just created:
|
||||||
//{
|
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
// sleep(50);
|
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
|
||||||
// idle();
|
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
|
||||||
//}
|
telemetry.addData("yaw",Yaw);
|
||||||
|
|
||||||
telemetry.addData("Mode", "waiting for start");
|
telemetry.update();
|
||||||
//telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
|
}
|
||||||
telemetry.update();
|
|
||||||
waitForStart();
|
|
||||||
|
|
||||||
//test
|
|
||||||
while (opModeIsActive()) {
|
|
||||||
x = gamepad1.left_stick_x;
|
|
||||||
y = gamepad1.left_stick_y;
|
|
||||||
t= gamepad1.right_trigger;
|
|
||||||
t2 = helloexp(t);
|
|
||||||
t3 = helloexp(Math.sqrt(Math.pow(x,2)+Math.pow(y,2)));
|
|
||||||
telemetry.addData("Status", "Running");
|
|
||||||
if(gamepad1.a && !already_a){
|
|
||||||
if(mode=="normal"){
|
|
||||||
mode="tank";
|
|
||||||
}else if(mode=="tank"){
|
|
||||||
mode = "essaifranck";
|
|
||||||
}else if (mode == "essaifranck"){
|
|
||||||
mode = "elina";
|
|
||||||
}else{
|
|
||||||
mode="normal";
|
|
||||||
}
|
|
||||||
already_a = true;
|
|
||||||
}
|
|
||||||
if(!gamepad1.a && already_a){
|
|
||||||
already_a = false;
|
|
||||||
}
|
|
||||||
double lpower = 0.0;
|
|
||||||
double rpower = 0.0;
|
|
||||||
if(mode=="normal"){
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
else if (mode=="tank"){
|
|
||||||
lpower = -y;
|
|
||||||
rpower = gamepad1.right_stick_y;
|
|
||||||
}
|
|
||||||
else if (mode=="essaifranck"){
|
|
||||||
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;
|
|
||||||
lpower = (a/vmean)*t2;
|
|
||||||
rpower = (b/vmean)*t2;
|
|
||||||
}
|
|
||||||
else if (mode=="elina"){
|
|
||||||
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;
|
|
||||||
lpower = (a/vmean)*t3;
|
|
||||||
rpower = (b/vmean)*t3;
|
|
||||||
}
|
|
||||||
if(!(gamepad1.left_bumper)){
|
|
||||||
lpower/=3;
|
|
||||||
rpower/=3;
|
|
||||||
}
|
|
||||||
lm.setPower(lpower);
|
|
||||||
rm.setPower(rpower);
|
|
||||||
|
|
||||||
if(gamepad1.b && !already_b){
|
|
||||||
already_b = !already_b;
|
|
||||||
if(moissoneuse.getPower() == 1.0){
|
|
||||||
moissoneuse.setPower(0);
|
|
||||||
}else{
|
|
||||||
moissoneuse.setPower(1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if(!gamepad1.b && already_b){
|
|
||||||
already_b = !already_b;
|
|
||||||
}
|
|
||||||
if(gamepad1.x && !already_x){
|
|
||||||
already_x = !already_x;
|
|
||||||
if(moissoneuse.getPower() == -1.0){
|
|
||||||
moissoneuse.setPower(0);
|
|
||||||
}else{
|
|
||||||
moissoneuse.setPower(-1);
|
|
||||||
}
|
|
||||||
if(!gamepad1.x && already_x){
|
|
||||||
already_x = !already_x;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
telemetry.addData("x",x);
|
|
||||||
telemetry.addData("y",y);
|
|
||||||
telemetry.addData("lpow",lpower);
|
|
||||||
telemetry.addData("rpow",rpower);
|
|
||||||
telemetry.addData("ltrigg",t);
|
|
||||||
telemetry.addData("t2",t2);
|
|
||||||
telemetry.addData("mode",mode);
|
|
||||||
// Create an object to receive the IMU angles
|
|
||||||
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();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue