werobot/ftc2024-robotcode
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge branch 'master' of https://forge.gzod01.fr/werobot/ftc2024-robotcode

Browse source
This commit is contained in:
GZod01 2024-04-06 14:48:10 +02:00
commit f87d734437
4 changed files with 125 additions and 107 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

130
FTC2024WeRobotControl.java
View file

@ -38,7 +38,7 @@ 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 defaultspeed = 0.6;
private ElapsedTime timer; private ElapsedTime timer;
@ -54,8 +54,8 @@ public class FTC2024WeRobotControl {
*/ */
public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) { public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) {
this.Parent = Parent; this.Parent = Parent;
this.timer = new ElapsedTime(); this.timer = new ElapsedTime();
} }
/* /*
@ -65,41 +65,28 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public void boxElv(){ public void boxElv() {
Parent.lmelevator.setVelocity(600); Parent.lmelevator.setVelocity(600);
Parent.rmelevator.setVelocity(600); Parent.rmelevator.setVelocity(600);
Parent.lmelevator.setTargetPosition(90);
Parent.rmelevator.setTargetPosition(90);
Parent.rotation.setVelocity(600); Parent.rotation.setVelocity(600);
Parent.lmelevator.setTargetPosition(50); Parent.rotation.setTargetPosition(20);
Parent.rmelevator.setTargetPosition(50);
Parent.rotation.setTargetPosition(30);
Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void boxElv() {
Parent.lmelevator.setVelocity(600);
Parent.rmelevator.setVelocity(600);
Parent.lmelevator.setTargetPosition(90);
Parent.rmelevator.setTargetPosition(90);
Parent.rotation.setVelocity(600);
Parent.rotation.setTargetPosition(-50);
Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
} }
public void posBasse() {
public void posBasse(){ Parent.lmelevator.setVelocity(600);
Parent.lmelevator.setVelocity(600); Parent.rmelevator.setVelocity(600);
Parent.rmelevator.setVelocity(600); Parent.rotation.setVelocity(600);
Parent.rotation.setVelocity(600); Parent.lmelevator.setTargetPosition(0);
Parent.lmelevator.setTargetPosition(0); Parent.rmelevator.setTargetPosition(0);
Parent.rmelevator.setTargetPosition(0); Parent.rotation.setTargetPosition(800);
Parent.rotation.setTargetPosition(800); Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
} }
/* /*
@ -110,12 +97,11 @@ public class FTC2024WeRobotControl {
*/ */
public double getSpeedFromMotorSpeed(double motor_speed) { public double getSpeedFromMotorSpeed(double motor_speed) {
double speed_tour_par_minutes = this.tour_par_minutes * motor_speed; double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter; double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter;
return speed; return speed;
} }
/* /*
* return the needed time for a distance * return the needed time for a distance
* *
@ -125,8 +111,8 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public double time_for_dist(double dist, double motor_speed) { public double time_for_dist(double dist, double motor_speed) {
double speed = getSpeedFromMotorSpeed(motor_speed); double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist / speed); return (dist / speed);
} }
/* /*
@ -139,14 +125,14 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public void forward(double n_tiles, double motor_speed) { public void forward(double n_tiles, double motor_speed) {
double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed); double total_time = time_for_dist(n_tiles * ground_tiles_width, Math.abs(motor_speed));
timer.reset(); timer.reset();
while (Parent.opModeIsActive() && timer.seconds() < total_time) { while (Parent.opModeIsActive() && timer.seconds() < total_time) {
Parent.lm.setPower(motor_speed); Parent.lm.setPower(motor_speed);
Parent.rm.setPower(motor_speed); Parent.rm.setPower(motor_speed);
} }
Parent.lm.setPower(0); Parent.lm.setPower(0);
Parent.rm.setPower(0); Parent.rm.setPower(0);
} }
/* /*
@ -156,7 +142,7 @@ public class FTC2024WeRobotControl {
* @param n_tiles number of tiles * @param n_tiles number of tiles
*/ */
public void forward(double n_tiles) { public void forward(double n_tiles) {
this.forward(n_tiles, this.defaultspeed); this.forward(n_tiles, this.defaultspeed);
} }
/* /*
@ -169,11 +155,11 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public void backward(double n_tiles, double motor_speed) { public void backward(double n_tiles, double motor_speed) {
forward(n_tiles, -motor_speed); forward(n_tiles, -motor_speed);
} }
public void backward(double n_tiles) { public void backward(double n_tiles) {
this.backward(n_tiles, this.defaultspeed); this.backward(n_tiles, this.defaultspeed);
} }
/* /*
@ -183,11 +169,11 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public void harvest(double motor_speed) { public void harvest(double motor_speed) {
Parent.harvestmotor.setPower(motor_speed); Parent.harvestmotor.setPower(motor_speed);
} }
public void harvest() { public void harvest() {
this.harvest(1.0); this.harvest(1.0);
} }
/* /*
@ -199,32 +185,32 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public void rotate(double angle, double motor_speed) { public void rotate(double angle, double motor_speed) {
robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double anglerad = Math.toRadians(angle); double anglerad = Math.toRadians(angle);
angle = Math.toDegrees(Math.atan2(Math.sin(anglerad), Math.cos(anglerad))); angle = Math.toDegrees(Math.atan2(Math.sin(anglerad), Math.cos(anglerad)));
double left_multiplier = -((double) Math.signum(angle)); double left_multiplier = -((double) Math.signum(angle));
double right_multiplier = ((double) Math.signum(angle)); double right_multiplier = ((double) Math.signum(angle));
double m_power = motor_speed; double m_power = motor_speed;
if (Math.abs(angle) == 180) { if (Math.abs(angle) == 180) {
angle = (double) (((double) Math.signum(angle)) * 179.9); angle = (double) (((double) Math.signum(angle)) * 179.9);
} }
while (Parent.opModeIsActive() while (Parent.opModeIsActive()
&& (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))) { && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))) {
robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double yaw = robotOrientation.getYaw(AngleUnit.DEGREES); double yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
Parent.telemetry.addData("Yaw", yaw); Parent.telemetry.addData("Yaw", yaw);
Parent.telemetry.update(); Parent.telemetry.update();
m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw));// relative m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw));// relative
Parent.lm.setPower(left_multiplier * m_power); Parent.lm.setPower(left_multiplier * m_power);
Parent.rm.setPower(right_multiplier * m_power); Parent.rm.setPower(right_multiplier * m_power);
} }
Parent.lm.setPower(0); Parent.lm.setPower(0);
Parent.rm.setPower(0); Parent.rm.setPower(0);
} }
public void rotate(double angle) { public void rotate(double angle) {
this.rotate(angle, this.defaultspeed); this.rotate(angle, this.defaultspeed);
} }
} }

21
ftc2024_auto_r2d.java
View file

@ -1,7 +1,18 @@
package org.firstinspires.ftc.teamcode;
import org.firstinspires.ftc.teamcode.Ftc2024_autonome_api;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
package org.firstinspires.ftc.teamcode; @Autonomous
import org.firstinspires.ftc.teamcode.Ftc2024_autonome_api; public class ftc2024_auto_r2d extends LinearOpMode{
public class ftc2024_auto_r2d extends Ftc2024_autonome_api{ //hi
public AutoMode autonomous_mode = AutoMode.R2D; public Ftc2024_autonome_api.AutoMode autonomous_mode = Ftc2024_autonome_api.AutoMode.R2D;
}
@Override
public void runOpMode() {
Ftc2024_autonome_api a = new Ftc2024_autonome_api();
a.hardwareMap = this.hardwareMap;
a.runOpMode();
}
}

68
ftc2024_autonome_api.java
View file

@ -19,28 +19,29 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcode.hardware.HardwareMap;
@Autonomous @Autonomous
public class Ftc2024_autonome_api extends LinearOpMode { public class Ftc2024_autonome_api{
public enum AutoMode{ public enum AutoMode {
B2D,B4D,B2N,B4N,R2D,R4D,R2N,R4N B2D, B4D, B2N, B4N, R2D, R4D, R2N, R4N
} }
public HardwareMap hardwareMap;
public AutoMode autonomous_mode; public AutoMode autonomous_mode;
public DcMotor lm; public DcMotorEx lm;
public DcMotor rm; public DcMotorEx rm;
public DcMotorEx lmelevator; public DcMotorEx lmelevator;
public DcMotorEx rmelevator; public DcMotorEx rmelevator;
public DcMotor harvestmotor; public DcMotor harvestmotor;
public IMU imu; public IMU imu;
public DcMotorEx rotation; public DcMotorEx rotation;
@Override
public void runOpMode() { public void runOpMode() {
boolean auto = false; boolean auto = false;
lm = hardwareMap.get(DcMotor.class, "blm"); lm = hardwareMap.get(DcMotorEx.class, "blm");
rm = hardwareMap.get(DcMotor.class, "brm"); rm = hardwareMap.get(DcMotorEx.class, "brm");
harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse"); harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse");
rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
@ -63,25 +64,24 @@ public class Ftc2024_autonome_api extends LinearOpMode {
imu.resetYaw(); imu.resetYaw();
YawPitchRollAngles robotOrientation; YawPitchRollAngles robotOrientation;
FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this); FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this);
autonomous_mode = AutoMode.B4D;
telemetry.addData("wait for start", ""); telemetry.addData("wait for start", "");
telemetry.update(); telemetry.update();
while (opModeInInit()){
imu.resetYaw();
waitForStart(); telemetry.addData("wait", "for start");
telemetry.addData("started", ""); telemetry.addData("Yaw", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
telemetry.update(); telemetry.update();
robotOrientation = imu.getRobotYawPitchRollAngles(); }
while (opModeIsActive()) { while (opModeIsActive()) {
if (gamepad1.a && !auto){ if (gamepad1.a && !auto) {
auto = true; auto = true;
break; break;
} }
} }
if (opModeIsActive()){ if (opModeIsActive()) {
/* /*
* autonomous_mode differents possibles values respect the next scheme: * autonomous_mode differents possibles values respect the next scheme:
* team_color_shortcode + start_line_index + direct_or_no * team_color_shortcode + start_line_index + direct_or_no
@ -103,6 +103,10 @@ public class Ftc2024_autonome_api extends LinearOpMode {
/*robot.harvest(0); /*robot.harvest(0);
robot.rotate((-90)); robot.rotate((-90));
robot.posBasse(); robot.posBasse();
while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
telemetry.addData("pos", rotation.getCurrentPosition());
telemetry.update();
}
robot.forward(3.5); robot.forward(3.5);
robot.harvest(-1); robot.harvest(-1);
robot.backward(0.5); robot.backward(0.5);
@ -110,7 +114,14 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.forward(0.5);*/ robot.forward(0.5);*/
break; break;
case B2D: case B2D:
imu.resetYaw();
robot.posBasse(); robot.posBasse();
while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
telemetry.addData("pos", rotation.getCurrentPosition());
telemetry.addData("Yaw", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
telemetry.update();
}
robot.harvest(1); robot.harvest(1);
robot.forward(1); robot.forward(1);
robot.harvest(0); robot.harvest(0);
@ -127,6 +138,10 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.harvest(0); robot.harvest(0);
robot.rotate(90); robot.rotate(90);
robot.posBasse(); robot.posBasse();
while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
telemetry.addData("pos", rotation.getCurrentPosition());
telemetry.update();
}
robot.forward(3.5); robot.forward(3.5);
robot.harvest(-1); robot.harvest(-1);
robot.backward(0.5); robot.backward(0.5);
@ -135,16 +150,21 @@ public class Ftc2024_autonome_api extends LinearOpMode {
break; break;
case R2D: case R2D:
robot.boxElv(); robot.boxElv();
robot.harvest(1); //rebuild
robot.forward(0.5); //robot.harvest(1);
robot.harvest(0); robot.forward(1.5);
//robot.harvest(0);
robot.rotate(90); robot.rotate(90);
robot.posBasse(); robot.posBasse();
while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
telemetry.addData("pos", rotation.getCurrentPosition());
telemetry.update();
}
robot.forward(1.5); robot.forward(1.5);
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 B4N: case B4N:
robot.boxElv(); robot.boxElv();

3
ftc_new.java
View file

@ -181,6 +181,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2))); t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)));
telemetry.addData("Status", "Running"); telemetry.addData("Status", "Running");
telemetry.addData("mode", mode);
if (gamepad1.right_bumper && gamepad1.left_bumper){ if (gamepad1.right_bumper && gamepad1.left_bumper){
@ -556,7 +557,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
//telemetry.addData("Position rotation", rotation.getCurrentPosition()); //telemetry.addData("Position rotation", rotation.getCurrentPosition());
//telemetry.addData("Position box", box.getCurrentPosition()); //telemetry.addData("Position box", box.getCurrentPosition());
//telemetry.addData("box velocity", rotation.getVelocity()); //telemetry.addData("box velocity", rotation.getVelocity());
//telemetry.update(); telemetry.update();
} }
} }