From 362332b6a5d6c243dd05d2de77c69007250f7d9e Mon Sep 17 00:00:00 2001 From: Gzod01 Date: Sat, 7 Oct 2023 09:41:04 +0000 Subject: [PATCH] Actualiser WeRobot-Code.java --- WeRobot-Code.java | 204 +++++++++++++++++++++++++++++++++++++--------- 1 file changed, 165 insertions(+), 39 deletions(-) diff --git a/WeRobot-Code.java b/WeRobot-Code.java index 2a6cd02..254ddb6 100644 --- a/WeRobot-Code.java +++ b/WeRobot-Code.java @@ -2,10 +2,14 @@ package org.firstinspires.ftc.teamcode; //package fr.gzod01.werobot.competition_singapour_2023; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +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: FGC2023", group="WeRobot") public class WeRobot_FGC2023 extends LinearOpMode { @@ -15,18 +19,32 @@ public class WeRobot_FGC2023 extends LinearOpMode { private DcMotor blm; // OTHER THAN MOVES: // lanceur = panier - private DcMotor lanceur = null; + private DcMotorEx panier1 = null; + private DcMotorEx panier2 = null; private DcMotor moissoneuse = null; private DcMotor accroche = null; + private Servo servoAccroche = null; + private Servo servoPanier = null; private double puissanceMoissoneuse = 0; - private boolean lanceurEnRoute = false; + private boolean panierEnRoute = false; private boolean dejavu = false; + private boolean dejavulb = false; + private boolean dejavudl = false; + private boolean dejavudr = false; private boolean modeFrein = false; private boolean moissoneuseEnRoute = false; + private boolean servoAccrocheEnRoute = true; private boolean dejavuM = false; + private boolean dejavuA = false; + private boolean dejavudown = false; + private boolean panierDescendu = false; + private double puissancePanier1 = 0; + private double puissancePanier2 = 0; + private int targetPos; + private Long targetPosLong; @Override - public void runOpMode() { + public void runOpMode() throws InterruptedException { float x; double y; float g2x; @@ -37,13 +55,25 @@ public class WeRobot_FGC2023 extends LinearOpMode { blm = hardwareMap.get(DcMotor.class, "blm"); brm = hardwareMap.get(DcMotor.class, "brm"); // OTHER THAN MOVES: - lanceur = hardwareMap.get(DcMotor.class, "lnc"); + panier1 = (DcMotorEx) hardwareMap.get(DcMotorEx.class, "pn1"); + panier2 = (DcMotorEx) hardwareMap.get(DcMotorEx.class, "pn2"); + + panier2.setDirection(DcMotorSimple.Direction.REVERSE); + + panier1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + panier2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + moissoneuse = hardwareMap.get(DcMotor.class, "msn"); accroche = hardwareMap.get(DcMotor.class,"mnt"); + servoAccroche = hardwareMap.get(Servo.class, "servomnt"); + servoPanier = hardwareMap.get(Servo.class, "servobascule"); accroche.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - WRRobot wr = new WRRobot(flm,frm,blm,brm); + WRRobot wr = new WRRobot(flm,frm,blm,brm,gamepad1); + servoPanier.setPosition(0.5); + servoAccroche.setPosition(0.5); waitForStart(); - + + while (opModeIsActive()) { telemetry.addData("Status", "Running"); telemetry.update(); @@ -53,29 +83,115 @@ public class WeRobot_FGC2023 extends LinearOpMode { wr.moves(x,y ); wr.Rotation(g2x ); // OTHER THAN MOVES: - // // bouton O - if (gamepad1.b){ + if (gamepad1.dpad_up){ if (!dejavu){ - if (lanceurEnRoute){ - lanceur.setPower(0.8); - } - else{ - lanceur.setPower(0); - } - lanceurEnRoute=!lanceurEnRoute; + // tick par seconde + + // panier1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + // panier2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + panier1.setVelocity(600); + panier2.setVelocity(600); + + // 288 ticks par tour + targetPosLong = (Long) Math.round(288*8.5); + telemetry.addData("Long Pos : ", targetPosLong); + + targetPos = targetPosLong.intValue(); + panier1.setTargetPosition(targetPos); + panier2.setTargetPosition(targetPos); + panier2.setMode(DcMotor.RunMode.RUN_TO_POSITION); + panier1.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // while (panier1.isBusy()){ + // targetPos = panier1.getCurrentPosition(); + + + // } + + telemetry.addData("New code :","isOK"); + + + + panierEnRoute=!panierEnRoute; dejavu=true; } } else{ - dejavu = false; + dejavu=false; } - //bouton X - if (gamepad1.a){ + if (gamepad1.dpad_down){ + if (!dejavudown){ + panier1.setVelocity(75); + panier2.setVelocity(75); + targetPosLong = (Long) Math.round(288*2.5); + targetPos = targetPosLong.intValue(); + panier1.setTargetPosition(targetPos); + panier2.setTargetPosition(targetPos); + dejavudown = true; + } + } + else{ + dejavudown = false; + } + + if (gamepad1.left_bumper){ + if(!dejavulb){ + double[] positions_to_go = {0.5,0.6,0.7,0.8,0.825,0.85,0.875,0.9,0.95,1}; + int[] times_to_sleep = { 100,100,100,200,200,200,200,200,200,200,200,200,200,200}; + for(int i= 0; i < positions_to_go.length; i++){ + Thread.sleep(times_to_sleep[i]); + servoPanier.setPosition(positions_to_go[i]); + } + dejavulb = true; + } + dejavulb = false; + } + + //boutton rose + if(gamepad1.x){ + if(!dejavudl){ + servoPanier.setPosition(0.37); + dejavudl = true; + } + dejavudl = false; + } + + if (gamepad1.right_bumper){ + if (!dejavuA){ + if (servoAccrocheEnRoute){ + /*double[] p_t_g = {0.55,0.5,0.45,0.4,0.35,0.3,0.275,0.2625,0.25,0.24}; + int[] t_t_s = {100,100,100,100,100,100,300,300,200,100}; + for(int i = 0; i< p_t_g.length; i++){ + Thread.sleep(t_t_s[i]); + servoAccroche.setPosition(p_t_g[i]); + telemetry.addData("puissance servo",servoAccroche.getPosition()); + }*/ + servoAccroche.setPosition(0.15); + + } + else { + /*double[] p_t_g = {0.3,0.325,0.35,0.375,0.4,0.425,0.45,0.5}; + int[] t_t_s = {100,100,100,100,100,200,200,100}; + for(int i = 0; i< p_t_g.length; i++){ + Thread.sleep(t_t_s[i]); + servoAccroche.setPosition(p_t_g[i]); + }*/ + servoAccroche.setPosition(0.5); + } + servoAccrocheEnRoute=!servoAccrocheEnRoute; + dejavuA = true; + } + } + else{ + dejavuA = false; + } + //bouton O + if (gamepad1.b){ if (!dejavuM){ if (moissoneuseEnRoute){ - moissoneuse.setPower(-1); + moissoneuse.setPower(-0.9); } - else{ + else{ moissoneuse.setPower(0); } moissoneuseEnRoute=!moissoneuseEnRoute; @@ -85,13 +201,14 @@ public class WeRobot_FGC2023 extends LinearOpMode { else{ dejavuM = false; } - if (gamepad1.right_bumper){ - accroche.setPower(0.8); - } - else if (gamepad1.left_bumper) - { + // bouton vert + if (gamepad1.a){ accroche.setPower(-0.8); } + // bouton X + else if (gamepad1.y){ + accroche.setPower(0.8); + } else { if (!modeFrein){ accroche.setPower(0); @@ -102,8 +219,18 @@ public class WeRobot_FGC2023 extends LinearOpMode { accroche.setPower(0.17); } puissanceMoissoneuse = moissoneuse.getPower(); - telemetry.addData("puissance moisso : ", puissanceMoissoneuse); - telemetry.addData("dejavu :", dejavu); + puissancePanier1 = panier1.getPower(); + puissancePanier2 = panier2.getPower(); + telemetry.addData("puissance moisso", puissanceMoissoneuse); + telemetry.addData("angle servo", servoAccroche.getPosition()); + telemetry.addData("dejavu", dejavu); + telemetry.addData("puissance panier1", puissancePanier1); + telemetry.addData("puissance panier2", puissancePanier2); + telemetry.addData("TUTORIEL","TUTORIEL"); + telemetry.addData("Stick Gauche","Translations"); + telemetry.addData("Stick Droit","Rotations"); + telemetry.addData("X","Descendre Crochet"); + telemetry.addData("Rond","Moissoneuse"); } } } @@ -112,11 +239,13 @@ class WRRobot{ private DcMotor frm; private DcMotor blm; private DcMotor brm; - public WRRobot(DcMotor cflm,DcMotor cfrm,DcMotor cblm,DcMotor cbrm){ + private Gamepad gamepad1; + public WRRobot(DcMotor cflm,DcMotor cfrm,DcMotor cblm,DcMotor cbrm,Gamepad gamepad1){ flm = cflm; frm = cfrm; blm = cblm; brm = cbrm; + this.gamepad1 = gamepad1; } public void moves(float x, double y){ double approx = 0.15; @@ -144,7 +273,7 @@ class WRRobot{ } public void diagonal(float x, double y){ - double pow = Math.abs(x+y)/2; + double pow = gamepad1.left_trigger > 0.5?gamepad1.left_trigger:(gamepad1.right_trigger>0.5?0.2:0.4); if(x>0){ if(y>0){ // North East @@ -166,7 +295,7 @@ class WRRobot{ } } public void horizontal(float x){ - double pow = Math.abs(x); + double pow = gamepad1.left_trigger > 0.5?gamepad1.left_trigger:(gamepad1.right_trigger>0.5?0.2:0.4); if(x>0){ movement(new double[]{pow,pow,-pow,-pow}); }else{ @@ -177,15 +306,15 @@ class WRRobot{ } public void vertical(double y){ - double pow = Math.abs(y); + double pow = gamepad1.left_trigger > 0.5?gamepad1.left_trigger:(gamepad1.right_trigger>0.5?0.2:0.4); if(y>0){ - movement(new double[]{pow,-pow,pow,-pow}); + movement(new double[]{pow,-pow,pow,-pow*1}); }else{ - movement(new double[]{-pow,pow,-pow,pow}); + movement(new double[]{-pow,pow,-pow,pow*1}); } } - public void movement(double[] list){ + public void movement(double[] list){ flm.setPower(list[0]); frm.setPower(list[1]); blm.setPower(list[2]); @@ -198,15 +327,12 @@ class WRRobot{ public void Rotation(float g2x){ double approx = 0.15; boolean xnozero = (g2x<-approx || g2x>approx); - double pow = Math.abs(g2x); + double pow = gamepad1.left_trigger > 0.5?gamepad1.left_trigger:(gamepad1.right_trigger>0.5?0.2:0.4); if (!xnozero) return; if (g2x > 0) { movement(new double[]{pow,pow,pow,pow}); } else { - movement(new double[]{-pow,-pow,-pow,pow}); + movement(new double[]{-pow,-pow,-pow,-pow}); } } - - } -