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

Merge remote-tracking branch 'origin/HEAD'

Browse source
This commit is contained in:
GZod01 2024-03-10 17:18:07 +01:00
commit f4fd16db72
2 changed files with 165 additions and 173 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

56
FTC2024WeRobotControl.java
View file

@ -1,4 +1,4 @@
package fr.werobot.ftc2024.robotcontrol;//a tester car pas sur que ça fonctionne package org.firstinspires.ftc.teamcode;//a tester car pas sur que ça fonctionne
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@ -21,7 +21,7 @@ public class FTC2024WeRobotControl {
/* /*
* The Parent class {@see FTC2024WeRobotControl constructor} * The Parent class {@see FTC2024WeRobotControl constructor}
*/ */
private LinearOpMode Parent; private Ftc2024_autonome_api Parent;
/* /*
* the wheel width in metres * the wheel width in metres
*/ */
@ -38,6 +38,11 @@ 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 ElapsedTime timer;
private YawPitchRollAngles robotOrientation;
/* /*
* construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class * construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class
* for the FTC2024 * for the FTC2024
@ -46,17 +51,11 @@ public class FTC2024WeRobotControl {
* constructing the class directly in * constructing the class directly in
*/ */
public FTC2024WeRobotControl(LinearOpMode Parent) { public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) {
this.Parent = Parent; this.Parent = Parent;
this.timer = new ElapsedTime();
} }
/*
* test if motorSpeed is >= 0
*/
public void testMotorSpeed(double motor_speed){
if(!((double)motor_speed>=0.0)){
throw new Exception("Motor Speed MUST be >= 0");
}
}
/* /*
* return a metre/sec speed * return a metre/sec speed
* *
@ -64,14 +63,10 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public double getSpeedFromMotorSpeed(double motor_speed) { public double getSpeedFromMotorSpeed(double motor_speed) {
testMotorSpeed(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;
} }
public double getSpeedFromMotorSpeed(){
this.getSpeedFromMotorSpeed(1);
}
/* /*
* return the needed time for a distance * return the needed time for a distance
@ -82,13 +77,9 @@ 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) {
testMotorSpeed(motor_speed);
double speed = getSpeedFromMotorSpeed(motor_speed); double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist / speed); return (dist / speed);
} }
public double time_for_dist(double dist){
this.time_for_dist(dist,1);
}
/* /*
* go forward * go forward
@ -100,10 +91,9 @@ 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) {
testMotorSpeed(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, motor_speed);
double start_time = Parent.runtime.seconds(); timer.reset();
while (Parent.opModeIsActive() && ((Parent.runtime.seconds() - start_time) < 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);
} }
@ -125,11 +115,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) {
testMotorSpeed(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.forward(n_tiles,1); this.backward(n_tiles,1);
} }
/* /*
@ -141,9 +131,7 @@ public class FTC2024WeRobotControl {
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(){
this.harvest(1);
}
/* /*
* harvest * harvest
* *
@ -153,24 +141,24 @@ public class FTC2024WeRobotControl {
* to 1 * to 1
*/ */
public void rotate(double angle, double motor_speed){ public void rotate(double angle, double motor_speed){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = Parent.robotOrientation.getYaw(AngleUnit.DEGREES); double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
angle = 200.0; angle = 200.0;
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;
while(Parent.opModeIsActive() && (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){ while(Parent.opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
m_power = (Math.abs(Parent.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(douvle angle){ public void rotate(double angle){
this.rotate(tour_par_minutes, 1); this.rotate(angle,1.0);
} }
} }

32
ftc2024_autonome_api.java
View file

@ -1,6 +1,6 @@
package org.firstinspires.ftc.teamcode; package org.firstinspires.ftc.teamcode;
import fr.werobot.ftc2024.robotcontrol.FTC2024WeRobotControl; //a tester car pas sur que ça fonctionne //import FTC2024WeRobotControl; //a tester car pas sur que ça fonctionne
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@ -21,13 +21,12 @@ import com.qualcomm.robotcore.util.ElapsedTime;
@Autonomous @Autonomous
public class ftc2024_autonome_api extends LinearOpMode { public class Ftc2024_autonome_api extends LinearOpMode {
public String autonomous_mode; public String autonomous_mode;
public DcMotor lm; public DcMotor lm;
public DcMotor rm; public DcMotor rm;
public DcMotor harvestmotor; public DcMotor harvestmotor;
public IMU imu; public IMU imu;
public YawPitchRollAngle robotOrientation;
@Override @Override
public void runOpMode() { public void runOpMode() {
@ -42,29 +41,34 @@ public class ftc2024_autonome_api extends LinearOpMode {
new IMU.Parameters( new IMU.Parameters(
new RevHubOrientationOnRobot( new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
)
)
);
imu.resetYaw(); imu.resetYaw();
// YawPitchRollAngle robotOrientation; // FTC2024WeRobotControl à besoin de robotOrientation comme variable de Parent (donc de cette classe actuelle), n'est il pas posible de définir robotOrientation dans la classe? YawPitchRollAngles robotOrientation;
FTC2024WeRobotControl robot = FTC2024WeRobotControl(this); FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this);
autonomous_mode = "b4d";
telemetry.addData("wait for start", "");
telemetry.update();
waitForStart(); waitForStart();
robotOrientation = imu.getYawPitchRollAngles(); telemetry.addData("started", "");
telemetry.update();
robotOrientation = imu.getRobotYawPitchRollAngles();
if(opModeIsRunning()){ if (opModeIsRunning()) {
/* /*
* 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
* *
* team_color_shortcode = "b" for blue & "r" for red * team_color_shortcode = "b" for blue & "r" for red
* start_line_index = 4 or 2 see competition manual appendix B Tile location plan * start_line_index = 4 or 2 see competition manual appendix B Tile location
* direct_or_no = "d" to direct go to pixel deliver zone or "n" to harvest pixels before to go in deliver zone * plan
* direct_or_no = "d" to direct go to pixel deliver zone or "n" to harvest
* pixels before to go in deliver zone
* *
* default is "b4d" * default is "b4d"
*/ */
switch (autonomous_mode){ switch (autonomous_mode) {
default: default:
robot.forward(0.5); robot.forward(0.5);
robot.rotate((-90)); robot.rotate((-90));