diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java new file mode 100644 index 0000000..88a5c96 --- /dev/null +++ b/FTC2024WeRobotControl.java @@ -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)