Merge branch 'master' of github.com:GZod01/ftc2024-robotcode
This commit is contained in:
commit
8fd698c3b6
1 changed files with 258 additions and 262 deletions
|
|
@ -25,122 +25,120 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
|||
//test
|
||||
@TeleOp(name = "WeRobot: FTC2024 Carlike", group = "WeRobot")
|
||||
public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||
/*
|
||||
* Le moteur de droite
|
||||
*/
|
||||
|
||||
// Le moteur de droite
|
||||
private DcMotor rm;
|
||||
/*
|
||||
* Le moteur de gauche
|
||||
*/
|
||||
|
||||
// Le moteur de gauche
|
||||
private DcMotor lm;
|
||||
/*
|
||||
* La moissoneuse
|
||||
*/
|
||||
|
||||
private DcMotor moissoneuse;
|
||||
/*
|
||||
* L'IMU
|
||||
*/
|
||||
|
||||
private DcMotor lmelevator;
|
||||
|
||||
private DcMotor rmelevator;
|
||||
|
||||
private DcMotor box;
|
||||
|
||||
private DcMotor rotation;
|
||||
|
||||
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) {
|
||||
return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1);
|
||||
}
|
||||
|
||||
private void tele(string name, string data) {
|
||||
telemetry.addData(name, data);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
/*
|
||||
* La fonction du thread principal
|
||||
*/
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
/*
|
||||
* l'axe x du joystick gauche de la manette
|
||||
*/
|
||||
// l'axe x du joystick gauche de la manette
|
||||
float x;
|
||||
/*
|
||||
* l'axe y du joystick gauche de la manette
|
||||
*/
|
||||
double y;
|
||||
/*
|
||||
* variation 1 du left trigger
|
||||
*/
|
||||
double t;
|
||||
/*
|
||||
* 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}
|
||||
*/
|
||||
// l'axe y du joystick gauche de la manette
|
||||
|
||||
double y;
|
||||
|
||||
// variation 1 du left trigger
|
||||
|
||||
double t;
|
||||
|
||||
// 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;
|
||||
|
||||
telemetry.addData("Status", "Initialized");
|
||||
|
||||
telemetry.update();
|
||||
|
||||
lm = hardwareMap.get(DcMotor.class, "blm");
|
||||
|
||||
rm = hardwareMap.get(DcMotor.class, "brm");
|
||||
|
||||
moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse");
|
||||
|
||||
lmelevator = hardwareMap.get(DcMotor.class, "ltrselv");
|
||||
lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
rmelevator = hardwareMap.get(DcMotor.class, "rtrselv");
|
||||
rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
rotation = hardwareMap.get(DcMotor.class, "elvRot");
|
||||
rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
box = hardwareMap.get(DcMotor.class, "boxRot");
|
||||
box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
imu = hardwareMap.get(IMU.class, "imu");
|
||||
/*
|
||||
* initialisation de l'imu
|
||||
*/
|
||||
|
||||
imu.initialize(
|
||||
/*
|
||||
* paramètres de l'imu
|
||||
*/
|
||||
|
||||
// paramètres de l'imu
|
||||
new IMU.Parameters(
|
||||
/*
|
||||
* orientation initiale du robot
|
||||
*/
|
||||
// orientation initiale du robot
|
||||
new RevHubOrientationOnRobot(
|
||||
/*
|
||||
* logo vers le haut
|
||||
*/
|
||||
//logo vers le haut
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
||||
/*
|
||||
* usb vers l'avant
|
||||
*/
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
|
||||
)
|
||||
)
|
||||
);
|
||||
/*
|
||||
* réinitialisation du yaw de l'imu
|
||||
*/
|
||||
//usb vers l'avant
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
|
||||
// réinitialisation du yaw de l'imu
|
||||
imu.resetYaw();
|
||||
// telemetry.addData("Mode", "calibrating...");
|
||||
// telemetry.update();
|
||||
|
|
@ -167,7 +165,8 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
|||
waitForStart();
|
||||
|
||||
/*
|
||||
* le robot a démarré, le tant que le robot est activé et donc qu'il n'a pas été stoppé:
|
||||
* le robot a démarré, le tant que le robot est activé et donc qu'il n'a pas été
|
||||
* stoppé:
|
||||
*/
|
||||
while (opModeIsActive()) {
|
||||
/*
|
||||
|
|
@ -183,11 +182,14 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
|||
*/
|
||||
t = gamepad1.right_trigger;
|
||||
/*
|
||||
* définition de {@link t2} par utilisation de la fonction {@link helloexp} sur {@link t}
|
||||
* 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é
|
||||
* 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)));
|
||||
/*
|
||||
|
|
@ -219,24 +221,25 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
|||
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"){
|
||||
tele("mode", "normal");
|
||||
} else if (mode == "tank") {
|
||||
lpower = -y;
|
||||
rpower = gamepad1.right_stick_y;
|
||||
}
|
||||
else if (mode=="essaifranck"){
|
||||
tele("mode", "tank");
|
||||
} 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"){
|
||||
tele("mode", "essai Franck");
|
||||
} 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;
|
||||
tele("mode", "Elina");
|
||||
}
|
||||
if (!(gamepad1.left_bumper)) {
|
||||
lpower /= 3;
|
||||
|
|
@ -246,27 +249,20 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
|||
rm.setPower(rpower);
|
||||
|
||||
if (gamepad1.b && !already_b) {
|
||||
double moissoneuseSpeed = 1.0;
|
||||
if (gamepad1.right_bumper){
|
||||
moissoneuseSpeed = -1.0;
|
||||
}
|
||||
already_b = !already_b;
|
||||
if(moissoneuse.getPower() == 1.0){
|
||||
if (moissoneuse.getPower() == moissoneuseSpeed) {
|
||||
moissoneuse.setPower(0);
|
||||
} else {
|
||||
moissoneuse.setPower(1);
|
||||
moissoneuse.setPower(moissoneuseSpeed);
|
||||
}
|
||||
}
|
||||
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);
|
||||
|
|
|
|||
Loading…
Reference in a new issue