Compare commits
13 commits
ec62678d80
...
5da5ebbe1e
Author | SHA1 | Date | |
---|---|---|---|
5da5ebbe1e | |||
|
8e3b2196a1 | ||
|
49caf3df3d | ||
|
335f8b3e89 | ||
eed163266b | |||
66925dc7e0 | |||
ee1a339a2a | |||
1eeff6a40c | |||
5907221497 | |||
|
7afcf0aa8d | ||
fb4690d606 | |||
f87d734437 | |||
878e91d992 |
5 changed files with 313 additions and 5 deletions
91
AutonomeTest.java
Normal file
91
AutonomeTest.java
Normal file
|
@ -0,0 +1,91 @@
|
||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
@Autonomous
|
||||||
|
|
||||||
|
public class AutonomeTest extends LinearOpMode {
|
||||||
|
private DcMotorEx droit;
|
||||||
|
private DcMotorEx gauche;
|
||||||
|
private DcMotorEx boiteG;
|
||||||
|
private DcMotorEx boiteD;
|
||||||
|
private ElapsedTime runtime = new ElapsedTime();
|
||||||
|
private DcMotor moissonneuse;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode(){
|
||||||
|
boiteD = hardwareMap.get(DcMotorEx.class, "rtrselv");
|
||||||
|
boiteD.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
|
boiteD.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
boiteD.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.FLOAT);
|
||||||
|
boiteG = hardwareMap.get(DcMotorEx.class, "ltrselv");
|
||||||
|
boiteG.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
boiteG.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.FLOAT);
|
||||||
|
gauche = hardwareMap.get(DcMotorEx.class, "blm");
|
||||||
|
gauche.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
gauche.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.FLOAT);
|
||||||
|
droit = hardwareMap.get(DcMotorEx.class, "brm");
|
||||||
|
droit.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
|
droit.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
droit.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.FLOAT);
|
||||||
|
moissonneuse = hardwareMap.get(DcMotor.class,"moissonneuse");
|
||||||
|
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
boiteG.setTargetPosition(90);
|
||||||
|
boiteD.setTargetPosition(90);
|
||||||
|
while (opModeIsActive() && boiteG.getCurrentPosition()<boiteG.getTargetPosition()){
|
||||||
|
boiteD.setVelocity(100);
|
||||||
|
boiteD.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
boiteG.setVelocity(100);
|
||||||
|
boiteG.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
telemetry.addData("position elv droit", boiteD.getCurrentPosition());
|
||||||
|
telemetry.addData("position elv gauche", boiteG.getCurrentPosition());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
gauche.setTargetPosition(700*3);
|
||||||
|
droit.setTargetPosition(700*3);
|
||||||
|
|
||||||
|
while (opModeIsActive() && gauche.getCurrentPosition()<gauche.getTargetPosition()){
|
||||||
|
gauche.setVelocity(250);
|
||||||
|
gauche.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
droit.setVelocity(250);
|
||||||
|
droit.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
gauche.setVelocity(0);
|
||||||
|
droit.setVelocity(0);
|
||||||
|
gauche.setTargetPosition(165);
|
||||||
|
droit.setTargetPosition(1235);
|
||||||
|
|
||||||
|
while (opModeIsActive() && droit.getCurrentPosition()>droit.getTargetPosition()){
|
||||||
|
gauche.setVelocity(250);
|
||||||
|
gauche.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
droit.setVelocity(250);
|
||||||
|
droit.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
gauche.setVelocity(0);
|
||||||
|
droit.setVelocity(0);
|
||||||
|
gauche.setTargetPosition(700*3);
|
||||||
|
droit.setTargetPosition(700*3);
|
||||||
|
|
||||||
|
while (opModeIsActive() && gauche.getCurrentPosition()<gauche.getTargetPosition()){
|
||||||
|
moissonneuse.setPower(1);
|
||||||
|
gauche.setVelocity(250);
|
||||||
|
gauche.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
droit.setVelocity(250);
|
||||||
|
droit.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
moissonneuse.setPower(0);
|
||||||
|
gauche.setVelocity(0);
|
||||||
|
droit.setVelocity(0);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
68
fgc2024Pilotage.java
Normal file
68
fgc2024Pilotage.java
Normal file
|
@ -0,0 +1,68 @@
|
||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
||||||
|
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;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.hardware.IMU;
|
||||||
|
import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
|
||||||
|
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||||
|
|
||||||
|
@TeleOp(name = "FGC2024", group = "WeRobot")
|
||||||
|
public class FGC_2024 extends LinearOpMode {
|
||||||
|
|
||||||
|
private DcMotorEx rm;
|
||||||
|
private DcMotorEx lm;
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
|
||||||
|
|
||||||
|
telemetry.addData("Status"," Initialized");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
lm = hardwareMap.get(DcMotorEx.class, "lm");
|
||||||
|
lm.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rm = hardwareMap.get(DcMotorEx.class, "rm");
|
||||||
|
//lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||||
|
//rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
while(opModeIsActive()){
|
||||||
|
float x = gamepad1.left_stick_x; // abscisse joystick gauche
|
||||||
|
double y = gamepad1.left_stick_y; // ordonnées joystick gauche
|
||||||
|
double lpower = 0.0; //puissance moteur gauche
|
||||||
|
double rpower = 0.0; //puissance moteur droit
|
||||||
|
|
||||||
|
lpower = ((1+x)*Math.signum(y))/2;
|
||||||
|
rpower = ((1-x)*Math.signum(y))/2;
|
||||||
|
|
||||||
|
if (Math.abs(x)>0.8){
|
||||||
|
lpower = 1*Math.signum(x);
|
||||||
|
rpower = -lpower;
|
||||||
|
}
|
||||||
|
|
||||||
|
lpower = lpower*gamepad1.left_trigger;
|
||||||
|
rpower = lpower*gamepad1.left_trigger;
|
||||||
|
|
||||||
|
rm.setPower(rpower);
|
||||||
|
lm.setPower(lpower);
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -100,7 +100,7 @@ public class Ftc2024_autonome_api{
|
||||||
robot.boxElv();
|
robot.boxElv();
|
||||||
robot.harvest(1);
|
robot.harvest(1);
|
||||||
robot.forward(2);
|
robot.forward(2);
|
||||||
robot.harvest(0);
|
/*robot.harvest(0);
|
||||||
robot.rotate((-90));
|
robot.rotate((-90));
|
||||||
robot.posBasse();
|
robot.posBasse();
|
||||||
while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
|
while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
|
||||||
|
@ -111,7 +111,7 @@ public class Ftc2024_autonome_api{
|
||||||
robot.harvest(-1);
|
robot.harvest(-1);
|
||||||
robot.backward(0.5);
|
robot.backward(0.5);
|
||||||
robot.harvest(0);
|
robot.harvest(0);
|
||||||
robot.forward(0.5);
|
robot.forward(0.5);*/
|
||||||
break;
|
break;
|
||||||
case B2D:
|
case B2D:
|
||||||
imu.resetYaw();
|
imu.resetYaw();
|
||||||
|
@ -123,13 +123,13 @@ public class Ftc2024_autonome_api{
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
robot.harvest(1);
|
robot.harvest(1);
|
||||||
robot.forward(0.5);
|
|
||||||
robot.harvest(0);
|
|
||||||
robot.forward(1);
|
robot.forward(1);
|
||||||
|
robot.harvest(0);
|
||||||
|
/* robot.forward(1);
|
||||||
robot.harvest(-1);
|
robot.harvest(-1);
|
||||||
robot.backward(0.5);
|
robot.backward(0.5);
|
||||||
robot.harvest(0);
|
robot.harvest(0);
|
||||||
robot.forward(0.5);
|
robot.forward(0.5);*/
|
||||||
break;
|
break;
|
||||||
case R4D:
|
case R4D:
|
||||||
robot.boxElv();
|
robot.boxElv();
|
||||||
|
|
1
hello.99.java
Normal file
1
hello.99.java
Normal file
|
@ -0,0 +1 @@
|
||||||
|
package tld.domain.truc;import tld.domain.autre_truc;public class hello_999{public void main(){System.out.println("helloworld");}}
|
148
lastmatch.java
Normal file
148
lastmatch.java
Normal file
|
@ -0,0 +1,148 @@
|
||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
//import FTC2024WeRobotControl; //a tester car pas sur que ça fonctionne
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.hardware.IMU;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
||||||
|
import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
|
||||||
|
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
@Autonomous
|
||||||
|
|
||||||
|
public class lastmatch extends LinearOpMode {
|
||||||
|
|
||||||
|
public DcMotorEx lm;
|
||||||
|
public DcMotorEx rm;
|
||||||
|
public DcMotorEx lmelevator;
|
||||||
|
public DcMotorEx rmelevator;
|
||||||
|
public DcMotor harvestmotor;
|
||||||
|
public IMU imu;
|
||||||
|
public DcMotorEx rotation;
|
||||||
|
private ElapsedTime timer;
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
timer = new ElapsedTime();
|
||||||
|
boolean auto = false;
|
||||||
|
lm = hardwareMap.get(DcMotorEx.class, "blm");
|
||||||
|
rm = hardwareMap.get(DcMotorEx.class, "brm");
|
||||||
|
harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse");
|
||||||
|
rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
|
||||||
|
rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
|
||||||
|
lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
|
||||||
|
rmelevator.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
|
rm.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
|
imu = hardwareMap.get(IMU.class, "imu");
|
||||||
|
imu.initialize(
|
||||||
|
new IMU.Parameters(
|
||||||
|
new RevHubOrientationOnRobot(
|
||||||
|
RevHubOrientationOnRobot.LogoFacingDirection.UP,
|
||||||
|
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
|
||||||
|
imu.resetYaw();
|
||||||
|
YawPitchRollAngles robotOrientation;
|
||||||
|
FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this);
|
||||||
|
|
||||||
|
telemetry.addData("wait for start", "");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
telemetry.addData("started", "");
|
||||||
|
telemetry.update();
|
||||||
|
robotOrientation = imu.getRobotYawPitchRollAngles();
|
||||||
|
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
if (gamepad1.a && !auto) {
|
||||||
|
auto = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (opModeIsActive()) {
|
||||||
|
double motor_speed = 1.0;
|
||||||
|
lmelevator.setVelocity(600);
|
||||||
|
rmelevator.setVelocity(600);
|
||||||
|
lmelevator.setTargetPosition(200);
|
||||||
|
rmelevator.setTargetPosition(200);
|
||||||
|
rotation.setVelocity(600);
|
||||||
|
rotation.setTargetPosition(40);
|
||||||
|
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
sleep(2000);
|
||||||
|
|
||||||
|
lm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
rm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
double total_time = robot.time_for_dist(1 * robot.ground_tiles_width, Math.abs(motor_speed));
|
||||||
|
timer.reset();
|
||||||
|
while (opModeIsActive() && timer.seconds() < total_time) {
|
||||||
|
lm.setPower(motor_speed);
|
||||||
|
rm.setPower(motor_speed);
|
||||||
|
}
|
||||||
|
lm.setPower(0);
|
||||||
|
rm.setPower(0);
|
||||||
|
|
||||||
|
harvestmotor.setPower(-0.6);
|
||||||
|
|
||||||
|
lm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
rm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
total_time = robot.time_for_dist(0.7 * robot.ground_tiles_width, Math.abs(motor_speed));
|
||||||
|
timer.reset();
|
||||||
|
while (opModeIsActive() && timer.seconds() < total_time) {
|
||||||
|
lm.setPower(-motor_speed);
|
||||||
|
rm.setPower(-motor_speed);
|
||||||
|
}
|
||||||
|
lm.setPower(0);
|
||||||
|
rm.setPower(0);
|
||||||
|
|
||||||
|
double angle = 90.0;
|
||||||
|
lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
double perimeter = Math.toRadians(angle)* 37.0/2.0;
|
||||||
|
int targetPos = (int) Math.floor(perimeter/(9e-2*Math.PI));
|
||||||
|
targetPos = targetPos * 20;
|
||||||
|
rm.setTargetPosition(targetPos);
|
||||||
|
lm.setTargetPosition(-targetPos);
|
||||||
|
lm.setVelocity(600);
|
||||||
|
rm.setVelocity(600);
|
||||||
|
lm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
// while (Math.abs(lm.getCurrentPosition() - targetPos)>=2){}
|
||||||
|
|
||||||
|
motor_speed= 1;
|
||||||
|
lm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
rm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
total_time = robot.time_for_dist(1.5 * robot.ground_tiles_width, Math.abs(motor_speed));
|
||||||
|
timer.reset();
|
||||||
|
while (opModeIsActive() && timer.seconds() < total_time) {
|
||||||
|
lm.setPower(motor_speed);
|
||||||
|
rm.setPower(motor_speed);
|
||||||
|
}
|
||||||
|
lm.setPower(0);
|
||||||
|
rm.setPower(0);
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
Loading…
Reference in a new issue