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;
@ -21,7 +21,7 @@ public class FTC2024WeRobotControl {
/*
* The Parent class {@see FTC2024WeRobotControl constructor}
*/
private LinearOpMode Parent;
private Ftc2024_autonome_api Parent;
/*
* the wheel width in metres
*/
@ -38,6 +38,11 @@ public class FTC2024WeRobotControl {
* the width size of the tiles on the ground in 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
* for the FTC2024
@ -46,17 +51,11 @@ public class FTC2024WeRobotControl {
* constructing the class directly in
*/
public FTC2024WeRobotControl(LinearOpMode Parent) {
public FTC2024WeRobotControl(Ftc2024_autonome_api 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
*
@ -64,14 +63,10 @@ public class FTC2024WeRobotControl {
* to 1
*/
public double getSpeedFromMotorSpeed(double motor_speed) {
testMotorSpeed(motor_speed);
double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter;
return speed;
}
public double getSpeedFromMotorSpeed(){
this.getSpeedFromMotorSpeed(1);
}
/*
* return the needed time for a distance
@ -82,13 +77,9 @@ public class FTC2024WeRobotControl {
* to 1
*/
public double time_for_dist(double dist, double motor_speed) {
testMotorSpeed(motor_speed);
double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist / speed);
}
public double time_for_dist(double dist){
this.time_for_dist(dist,1);
}
/*
* go forward
@ -100,10 +91,9 @@ public class FTC2024WeRobotControl {
* to 1
*/
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 start_time = Parent.runtime.seconds();
while (Parent.opModeIsActive() && ((Parent.runtime.seconds() - start_time) < total_time)) {
timer.reset();
while (Parent.opModeIsActive() && timer.seconds() < total_time) {
Parent.lm.setPower(motor_speed);
Parent.rm.setPower(motor_speed);
}
@ -125,11 +115,11 @@ public class FTC2024WeRobotControl {
* to 1
*/
public void backward(double n_tiles, double motor_speed) {
testMotorSpeed(motor_speed);
forward(n_tiles, -motor_speed);
}
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) {
Parent.harvestmotor.setPower(motor_speed);
}
public void harvest(){
this.harvest(1);
}
/*
* harvest
*
@ -153,24 +141,24 @@ public class FTC2024WeRobotControl {
* to 1
*/
public void rotate(double angle, double motor_speed){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = Parent.robotOrientation.getYaw(AngleUnit.DEGREES);
robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
angle = 200.0;
double anglerad = Math.toRadians(angle);
angle = Math.toDegrees(Math.atan2(Math.sin(anglerad),Math.cos(anglerad)));
double left_multiplier = -((double) Math.signum(angle));
double right_multiplier = ((double) Math.signum(angle));
double m_power = motor_speed;
while(Parent.opModeIsActive() && (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
m_power = (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative
while(Parent.opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){
robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative
Parent.lm.setPower(left_multiplier*m_power);
Parent.rm.setPower(right_multiplier*m_power);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
public void rotate(douvle angle){
this.rotate(tour_par_minutes, 1);
public void rotate(double angle){
this.rotate(angle,1.0);
}
}

32
ftc2024_autonome_api.java
View file

@ -1,6 +1,6 @@
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;
@ -21,13 +21,12 @@ import com.qualcomm.robotcore.util.ElapsedTime;
@Autonomous
public class ftc2024_autonome_api extends LinearOpMode {
public class Ftc2024_autonome_api extends LinearOpMode {
public String autonomous_mode;
public DcMotor lm;
public DcMotor rm;
public DcMotor harvestmotor;
public IMU imu;
public YawPitchRollAngle robotOrientation;
@Override
public void runOpMode() {
@ -42,29 +41,34 @@ public class ftc2024_autonome_api extends LinearOpMode {
new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)
)
);
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
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?
FTC2024WeRobotControl robot = FTC2024WeRobotControl(this);
YawPitchRollAngles robotOrientation;
FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this);
autonomous_mode = "b4d";
telemetry.addData("wait for start", "");
telemetry.update();
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:
* team_color_shortcode + start_line_index + direct_or_no
*
* team_color_shortcode = "b" for blue & "r" for red
* start_line_index = 4 or 2 see competition manual appendix B Tile location plan
* direct_or_no = "d" to direct go to pixel deliver zone or "n" to harvest pixels before to go in deliver zone
* start_line_index = 4 or 2 see competition manual appendix B Tile location
* 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"
*/
switch (autonomous_mode){
switch (autonomous_mode) {
default:
robot.forward(0.5);
robot.rotate((-90));