Merge branch 'master' of https://github.com/GZod01/ftc2024-robotcode
This commit is contained in:
Zelina974
commit
2f6da412ad
6 changed files with 315 additions and 247 deletions
|
|
@ -38,6 +38,9 @@ public class FTC2024WeRobotControl {
|
||||||
* the width size of the tiles on the ground in metres
|
* the width size of the tiles on the ground in metres
|
||||||
*/
|
*/
|
||||||
private final double ground_tiles_width = 61.0e-2; // metres
|
private final double ground_tiles_width = 61.0e-2; // metres
|
||||||
|
//
|
||||||
|
private final double defaultspeed = 0.6;
|
||||||
|
private final double defaultanglespeed = 0.4;
|
||||||
|
|
||||||
private ElapsedTime timer;
|
private ElapsedTime timer;
|
||||||
|
|
||||||
|
|
@ -102,7 +105,7 @@ public class FTC2024WeRobotControl {
|
||||||
}
|
}
|
||||||
|
|
||||||
public void forward(double n_tiles){
|
public void forward(double n_tiles){
|
||||||
this.forward(n_tiles,1);
|
this.forward(n_tiles,this.defaultspeed);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
@ -119,7 +122,7 @@ public class FTC2024WeRobotControl {
|
||||||
}
|
}
|
||||||
|
|
||||||
public void backward(double n_tiles){
|
public void backward(double n_tiles){
|
||||||
this.backward(n_tiles,1);
|
this.backward(n_tiles,this.defaultspeed);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
@ -167,7 +170,7 @@ public class FTC2024WeRobotControl {
|
||||||
Parent.rm.setPower(0);
|
Parent.rm.setPower(0);
|
||||||
}
|
}
|
||||||
public void rotate(double angle){
|
public void rotate(double angle){
|
||||||
this.rotate(angle,1.0);
|
this.rotate(angle,this.defaultanglespeed);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void test_forward_10_and_rotate_20deg(){
|
public void test_forward_10_and_rotate_20deg(){
|
||||||
|
|
|
||||||
|
|
@ -26,268 +26,323 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||||
@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
|
// Le moteur de droite
|
||||||
private DcMotor rm;
|
private DcMotor rm;
|
||||||
|
|
||||||
// Le moteur de gauche
|
// Le moteur de gauche
|
||||||
private DcMotor lm;
|
private DcMotor lm;
|
||||||
|
|
||||||
private DcMotor moissoneuse;
|
private DcMotor moissoneuse;
|
||||||
|
|
||||||
private DcMotorEx lmelevator;
|
private DcMotorEx lmelevator;
|
||||||
|
|
||||||
private DcMotorEx rmelevator;
|
private DcMotorEx rmelevator;
|
||||||
|
|
||||||
private DcMotorEx box;
|
private DcMotorEx box;
|
||||||
|
|
||||||
private DcMotorEx rotation;
|
private DcMotorEx rotation;
|
||||||
|
|
||||||
private IMU imu;
|
private IMU imu;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* La fonction pour faire des exponentielles spécifiques
|
* La fonction pour faire des exponentielles spécifiques
|
||||||
*
|
*
|
||||||
* @param double t => le nombre dont on veut faire l'exponentielle
|
* @param double t => le nombre dont on veut faire l'exponentielle
|
||||||
*
|
*
|
||||||
* @return double une_exponentielle_très_spéciale_de_t
|
* @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);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void tele(string name, string data) {
|
private void tele(String name, String data) {
|
||||||
telemetry.addData(name, data);
|
telemetry.addData(name, data);
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* La fonction du thread principal
|
* La fonction du thread principal
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() throws InterruptedException {
|
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;
|
float x;
|
||||||
|
|
||||||
// l'axe y du joystick gauche de la manette
|
// l'axe y du joystick gauche de la manette
|
||||||
|
|
||||||
double y;
|
double y;
|
||||||
|
|
||||||
// variation 1 du left trigger
|
// variation 1 du left trigger
|
||||||
|
|
||||||
double t;
|
double t;
|
||||||
|
|
||||||
// variation 2 du left trigger
|
// variation 2 du left trigger
|
||||||
|
|
||||||
double t2;
|
double t2;
|
||||||
|
|
||||||
// variation 3 du left trigger
|
// variation 3 du left trigger
|
||||||
|
|
||||||
double t3;
|
double t3;
|
||||||
|
|
||||||
// le mode du robot
|
// le mode du robot
|
||||||
|
|
||||||
String mode = "normal";
|
String mode = "elina";
|
||||||
|
|
||||||
// b est il déjà préssé?
|
// b est il déjà préssé?
|
||||||
|
|
||||||
boolean already_b = false;
|
boolean already_b = false;
|
||||||
|
|
||||||
// a est il déjà préssé?
|
boolean already_a = false;
|
||||||
|
|
||||||
boolean already_a = false;
|
boolean already_x = false;
|
||||||
|
|
||||||
|
boolean already_y = false;
|
||||||
|
|
||||||
// x est il déjà préssé?
|
boolean already_up = false;
|
||||||
|
|
||||||
boolean already_x = false;
|
boolean already_down = false;
|
||||||
|
|
||||||
|
boolean sinking = false;
|
||||||
|
|
||||||
boolean already_up = false;
|
telemetry.addData("Status", "Initialized");
|
||||||
|
|
||||||
boolean already_down = 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");
|
moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse");
|
||||||
|
|
||||||
rm = hardwareMap.get(DcMotor.class, "brm");
|
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
|
||||||
|
lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse");
|
rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
|
||||||
|
rmelevator.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
|
rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
|
||||||
lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
|
box = hardwareMap.get(DcMotorEx.class, "boxRot");
|
||||||
rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
|
imu = hardwareMap.get(IMU.class, "imu");
|
||||||
rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
// rotation positions: 20° pos initiale par rapport au sol
|
||||||
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rotation.setVelocity(700, AngleUnit.DEGREES);
|
||||||
|
rotation.setTargetPosition(40*288/360);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
imu.initialize(
|
||||||
|
|
||||||
box = hardwareMap.get(DcMotorEx.class, "boxRot");
|
// paramètres de l'imu
|
||||||
box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
new IMU.Parameters(
|
||||||
box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
// 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();
|
||||||
|
|
||||||
imu = hardwareMap.get(IMU.class, "imu");
|
// make sure the imu gyro is calibrated before continuing.
|
||||||
|
// while (!isStopRequested() && !imu.isGyroCalibrated())
|
||||||
|
// {
|
||||||
|
// sleep(50);
|
||||||
|
// idle();
|
||||||
|
// }
|
||||||
|
|
||||||
imu.initialize(
|
/*
|
||||||
|
* 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();
|
||||||
|
while (opModeIsActive()){
|
||||||
|
telemetry.addData("rotation target Pos", rotation.getTargetPosition());
|
||||||
|
telemetry.addData("rotation Pos", rotation.getCurrentPosition());
|
||||||
|
telemetry.addData("rotation velocity", rotation.getVelocity());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
/*
|
||||||
|
* 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 = "elina";
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
tele("mode", "normal");
|
||||||
|
} else if (mode == "tank") {
|
||||||
|
lpower = -y;
|
||||||
|
rpower = gamepad1.right_stick_y;
|
||||||
|
tele("mode", "tank");*/
|
||||||
|
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;
|
||||||
|
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_trigger>0.1) {
|
||||||
|
lpower /= 3;
|
||||||
|
rpower /= 3;
|
||||||
|
}
|
||||||
|
lm.setPower(lpower);
|
||||||
|
rm.setPower(rpower);
|
||||||
|
|
||||||
// paramètres de l'imu
|
if (gamepad1.b && !already_b) {
|
||||||
new IMU.Parameters(
|
double moissoneuseSpeed = 1.0;
|
||||||
// orientation initiale du robot
|
if (gamepad1.right_bumper){
|
||||||
new RevHubOrientationOnRobot(
|
moissoneuseSpeed = -1.0;
|
||||||
//logo vers le haut
|
}
|
||||||
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
already_b = !already_b;
|
||||||
//usb vers l'avant
|
if (moissoneuse.getPower() == moissoneuseSpeed) {
|
||||||
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
|
moissoneuse.setPower(0);
|
||||||
// réinitialisation du yaw de l'imu
|
} else {
|
||||||
imu.resetYaw();
|
moissoneuse.setPower(moissoneuseSpeed);
|
||||||
// telemetry.addData("Mode", "calibrating...");
|
}
|
||||||
// telemetry.update();
|
}
|
||||||
|
if (!gamepad1.b && already_b) {
|
||||||
// make sure the imu gyro is calibrated before continuing.
|
already_b = false;
|
||||||
// while (!isStopRequested() && !imu.isGyroCalibrated())
|
}
|
||||||
// {
|
|
||||||
// sleep(50);
|
if (sinking && Math.abs(lmelevator.getCurrentPosition()-90)<=5 && Math.abs(rmelevator.getCurrentPosition()-90)<=5){
|
||||||
// idle();
|
lmelevator.setVelocity(100);
|
||||||
// }
|
rmelevator.setVelocity(100);
|
||||||
|
lmelevator.setTargetPosition(0);
|
||||||
/*
|
rmelevator.setTargetPosition(0);
|
||||||
* ajout de la donnée en "mode": "en attente de démarrage" sur telemetry
|
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
*/
|
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
telemetry.addData("Mode", "waiting for start");
|
}
|
||||||
// telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
|
if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){
|
||||||
/*
|
lmelevator.setVelocity(600);
|
||||||
* mise à jour de la telemetry
|
rmelevator.setVelocity(600);
|
||||||
*/
|
Long targetPosLong = (Long) Math.round(288*3.4);
|
||||||
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;
|
|
||||||
tele("mode", "normal");
|
|
||||||
} else if (mode == "tank") {
|
|
||||||
lpower = -y;
|
|
||||||
rpower = gamepad1.right_stick_y;
|
|
||||||
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;
|
|
||||||
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;
|
|
||||||
rpower /= 3;
|
|
||||||
}
|
|
||||||
lm.setPower(lpower);
|
|
||||||
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() == moissoneuseSpeed) {
|
|
||||||
moissoneuse.setPower(0);
|
|
||||||
} else {
|
|
||||||
moissoneuse.setPower(moissoneuseSpeed);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (!gamepad1.b && already_b) {
|
|
||||||
already_b = false;
|
|
||||||
}
|
|
||||||
if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){
|
|
||||||
lmelevator.setVelocity(100);
|
|
||||||
rmelevator.setVelocity(100);
|
|
||||||
Long targetPosLong = (Long) Math.round(288*3.45);
|
|
||||||
int targetPos = targetPosLong.intValue();
|
int targetPos = targetPosLong.intValue();
|
||||||
if (gamepad1.dpad_down){
|
if (gamepad1.dpad_down){
|
||||||
targetPos = 0;
|
targetPos = 90;
|
||||||
already_down = true;
|
already_down = true;
|
||||||
}else{
|
sinking = true;
|
||||||
already_up = true;
|
}else{
|
||||||
}
|
already_up = true;
|
||||||
|
sinking = false;
|
||||||
|
}
|
||||||
lmelevator.setTargetPosition(targetPos);
|
lmelevator.setTargetPosition(targetPos);
|
||||||
rmelevator.setTargetPosition(targetPos);
|
rmelevator.setTargetPosition(targetPos);
|
||||||
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
}else if (!gamepad1.dpad_up && already_up){
|
|
||||||
already_up = false;
|
}else if (!gamepad1.dpad_up && already_up){
|
||||||
}else if (!gamepad1.dpad_down && already_down){
|
already_up = false;
|
||||||
already_down = false;
|
}else if (!gamepad1.dpad_down && already_down){
|
||||||
}
|
already_down = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (gamepad1.x && !already_x){
|
||||||
|
box.setVelocity(200);
|
||||||
|
int targetPos = 0;
|
||||||
|
if(gamepad1.right_bumper){
|
||||||
|
targetPos = 0;
|
||||||
|
}
|
||||||
|
box.setTargetPosition(targetPos);
|
||||||
|
box.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
already_x = !already_x;
|
||||||
|
} else if(!gamepad1.x && already_x){
|
||||||
|
already_x = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (gamepad1.y && already_y){
|
||||||
|
rotation.setVelocity(450);
|
||||||
|
int targetPos = 0;
|
||||||
|
if (gamepad1.right_bumper){
|
||||||
|
targetPos = 0;
|
||||||
|
}else if (gamepad1.left_bumper){
|
||||||
|
targetPos = 0;
|
||||||
|
}
|
||||||
|
rotation.setTargetPosition(targetPos);
|
||||||
|
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (gamepad1.right_bumper && gamepad1.left_bumper){
|
||||||
|
// launch the plane
|
||||||
|
}
|
||||||
|
|
||||||
|
<<<<<<< HEAD
|
||||||
telemetry.addData("x", x);
|
telemetry.addData("x", x);
|
||||||
telemetry.addData("y", y);
|
telemetry.addData("y", y);
|
||||||
telemetry.addData("lpow", lpower);
|
telemetry.addData("lpow", lpower);
|
||||||
|
|
@ -297,15 +352,36 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||||
// Create an object to receive the IMU angles
|
// Create an object to receive the IMU angles
|
||||||
YawPitchRollAngles robotOrientation;
|
YawPitchRollAngles robotOrientation;
|
||||||
robotOrientation = imu.getRobotYawPitchRollAngles();
|
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.addData("x", x);
|
||||||
|
telemetry.addData("y", y);
|
||||||
|
telemetry.addData("lpow", lpower);
|
||||||
|
telemetry.addData("rpow", rpower);
|
||||||
|
telemetry.addData("ltrigg", t);
|
||||||
|
telemetry.addData("t2", t2);
|
||||||
|
// Create an object to receive the IMU angles
|
||||||
|
YawPitchRollAngles robotOrientation;
|
||||||
|
robotOrientation = imu.getRobotYawPitchRollAngles();
|
||||||
|
>>>>>>> cdd50be8b9f970f1db99003d4c401bf7dafa25b6
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
<<<<<<< HEAD
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
=======
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
>>>>>>> cdd50be8b9f970f1db99003d4c401bf7dafa25b6
|
||||||
|
|
|
||||||
|
|
@ -32,7 +32,11 @@ public class Ftc2024_autonome_api extends LinearOpMode {
|
||||||
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");
|
||||||
harvestmotor = hardwareMap.get(DcMotor.class, "flm");
|
<<<<<<< HEAD
|
||||||
|
harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse");
|
||||||
|
=======
|
||||||
|
harvestmotor = hardwareMap.get(DcMotor.class, "moissoneuse");
|
||||||
|
>>>>>>> a8ae6d3 (hello)
|
||||||
|
|
||||||
rm.setDirection(DcMotor.Direction.REVERSE);
|
rm.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -329,10 +329,7 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||||
if (gamepad1.right_bumper && gamepad1.left_bumper){
|
if (gamepad1.right_bumper && gamepad1.left_bumper){
|
||||||
// launch the plane
|
// launch the plane
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
telemetry.addData("x", x);
|
telemetry.addData("x", x);
|
||||||
telemetry.addData("y", y);
|
telemetry.addData("y", y);
|
||||||
telemetry.addData("lpow", lpower);
|
telemetry.addData("lpow", lpower);
|
||||||
|
|
|
||||||
12
list.py
12
list.py
|
|
@ -1,12 +0,0 @@
|
||||||
|
|
||||||
|
|
||||||
def calcul_suite(n):
|
|
||||||
res1 = [1];
|
|
||||||
for i in range(n):
|
|
||||||
res2 = 0;
|
|
||||||
for j in res1:
|
|
||||||
res2+=j;
|
|
||||||
res2+=1;
|
|
||||||
res1.append(res2);
|
|
||||||
return res1[n-1];
|
|
||||||
|
|
||||||
|
|
@ -49,7 +49,7 @@ public class ftc2024_autonome_test extends LinearOpMode {
|
||||||
|
|
||||||
waitForStart();
|
waitForStart();
|
||||||
|
|
||||||
while (opModeIsActive()){
|
if (opModeIsActive()){
|
||||||
double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
|
double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
|
||||||
double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1};
|
double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1};
|
||||||
double [] y = new double[lm_p.length];
|
double [] y = new double[lm_p.length];
|
||||||
|
|
@ -99,4 +99,4 @@ public class ftc2024_autonome_test extends LinearOpMode {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue