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

helloworld

Browse source
This commit is contained in:
GZod01 2024-03-08 15:26:48 +01:00
parent 4a88d1faef
commit 2f72d41ed2
4 changed files with 154 additions and 83 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

119
FTC2024WeRobotControl.java Normal file
View file

@ -0,0 +1,119 @@
package fr.werobot.ftc2024.robotcontrol;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
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;
public class FTC2024WeRobotControl {
/*
* The Parent class {@see FTC2024WeRobotControl constructor}
*/
private LinearOpMode Parent;
/*
* the wheel width in metres
*/
private final double wheel_width = 9.0e-2; // metres
/*
* the wheel perimeter in meter
*/
private final double wheel_perimeter = Math.PI * wheel_width;
/*
* the rpm at max power of the motors
*/
private final double tour_par_minutes = 300.0;
/*
* the width size of the tiles on the ground in metres
*/
private final double ground_tiles_width = 61.0e-2; // metres
/*
* construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class for the FTC2024
* @param Parent = the parent class, use the "this" keyword if you are constructing the class directly in
*/
public FTC2024WeRobotControl(YawPitchhRollAngle Parent){
this.Parent = Parent;
}
/*
* return a metre/sec speed
* @param motor_speed = (optional) double between 0 and 1; motor power; default to 1
*/
public double getSpeedFromMotorSpeed(double motor_speed = 1.0){
double speed_tour_par_minutes = this.tour_par_minutes*motor_speed;
double speed = (speed_tour_par_minutes/60)*this.wheel_perimeter;
return speed;
}
/*
* return the needed time for a distance
* @param dist = distance in metre
* @param motor_speed = (optional) double between 0 and 1; motor power; default to 1
*/
public double time_for_dist(double dist, double motor_speed=1.0){
double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist/speed);
}
/*
* go forward
* @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 etc.)
* @param motor_speed = (optional) double between 0 and 1; motor power; default to 1
*/
public void forward(double n_tiles, double motor_speed = 1.0){
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)){
Parent.lm.setPower(motor_speed);
Parent.rm.setPower(motor_speed);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
/*
* go backward
* @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 etc.)
* @param motor_speed = (optional) double between 0 and 1; motor power; default to 1
*/
public void backward(double n_tiles, double motor_speed= 1.0){
forward(n_tiles, -motor_speed);
}
/*
* harvest
* @param motor_speed = (optional) double between 0 and 1; motor power; default to 1
*/
public void harvest(double motor_speed=0.0){
Parent.harvestmotor.setPower(motor_speed);
}
/*
* harvest
* @param angle = the angle to rotate (in degrees)
* @param motor_speed = (optional) double between 0 and 1; motor power; default to 1
*/
public void rotate(double angle, double motor_speed=1.0){
Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles();
double start_yaw = Parent.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
Parent.lm.setPower(left_multiplier*m_power);
Parent.rm.setPower(right_multiplier*m_power);
}
Parent.lm.setPower(0);
Parent.rm.setPower(0);
}
}

2
LICENSE
View file

@ -1,6 +1,6 @@
MIT License
Copyright (c) 2023 werobot
Copyright (c) 2024 WeRobot
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

115
autonome_api.java
View file

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode;
import fr.werobot.ftc2024.robotcontrol.FTC2024WeRobotControl;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.IMU;
@ -20,92 +22,41 @@ import com.qualcomm.robotcore.util.ElapsedTime;
@Autonomous
public class ftc2024_autonome_api extends LinearOpMode {
private DcMotor rm;
private DcMotor lm;
private DcMotor harvestmotor;
private IMU imu;
private final double wheel_width = 9.0e-2; // metres
private final double wheel_perimeter = Math.PI * wheel_width;
private final double tour_par_minutes = 300.0;
private final double ground_tiles_width = 61.0e-2; // metres
/*
* return a metre/sec speed
* @param motor_speed = double between 0 and 1
*/
public double getSpeedFromMotorSpeed(double motor_speed = 1.0){
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 time_for_dist(double dist, double motor_speed=1.0){
double speed = getSpeedFromMotorSpeed(motor_speed);
return (dist/speed);
}
public void forward(double n_tiles, double motor_speed = 1.0){
double total_time = time_for_dist(n_tiles*ground_tiles_width, motor_speed);
double start_time = runtime.seconds();
while( opModeIsActive() && ((runtime.seconds()-start_time)<total_time)){
lm.setPower(motor_speed);
rm.setPower(motor_speed);
}
lm.setPower(0);
rm.setPower(0);
}
public void backward(double n_tiles, double motor_speed= 1.0){
forward(n_tiles, -motor_speed);
}
public void harvest(double motor_speed=0.0){
harvestmotor.setPower(motor_speed);
}
public void rotate(double angle, double motor_speed=1.0){
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(opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle)){
m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative
lm.setPower(left_multiplier*m_power);
rm.setPower(right_multiplier*m_power);
}
lm.setPower(0);
rm.setPower(0);
}
public DcMotor lm;
public DcMotor rm;
public DcMotor harvestmotor;
public IMU imu;
public YawPitchRollAngle robotOrientation;
@Override
public void runOpMode() {
lm = hardwareMap.get (DcMotor.class, "blm");
rm = hardwareMap.get (DcMotor.class, "brm");
harvestmotor = hardwareMap.get(DcMotor.class, "flm");
lm = hardwareMap.get(DcMotor.class, "blm");
rm = hardwareMap.get(DcMotor.class, "brm");
harvestmotor = hardwareMap.get(DcMotor.class, "flm");
rm.setDirection(DcMotor.Direction.REVERSE);
imu = hardwareMap.get(IMU.class, "imu");
imu.initialize(
new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)
)
);
imu.resetYaw();
YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles();
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double yaw_sortie = 0.0;
rm.setDirection(DcMotor.Direction.REVERSE);
waitForStart();
imu = hardwareMap.get(IMU.class, "imu");
imu.initialize(
new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)
)
);
imu.resetYaw();
FTC2024WeRobotControl robot = FTC2024WeRobotControl(this);
if(opModeIsRunning()){
forward(0.5);
rotate(-90.0);
forward(1.5);
harvest(-1);
backward(1);
harvest(0);
waitForStart();
robotOrientation = imu.getYawPitchRollAngles();
if(opModeIsRunning()){
robot.forward(0.5);
robot.rotate(-90.0);
robot.forward(1.5);
robot.harvest(-1);
robot.backward(1);
robot.harvest(0);
}
}
}

1
ftc2024-autonome.java
View file

@ -42,6 +42,7 @@ public class ftc2024_autonome_test extends LinearOpMode {
)
);
imu.resetYaw();
YawPitchRollAngle robotOrientation;
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double yaw_sortie = 0.0;