hello
This commit is contained in:
parent
6d2ed0fb44
commit
2c42078e53
2 changed files with 214 additions and 213 deletions
|
|
@ -17,82 +17,82 @@ import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
@Autonomous
|
@Autonomous
|
||||||
|
|
||||||
public class ftc2024_autonome extends LinearOpMode {
|
public class ftc2024_autonome extends LinearOpMode {
|
||||||
private DcMotor rm;
|
private DcMotor rm;
|
||||||
private DcMotor lm;
|
private DcMotor lm;
|
||||||
private ElapsedTime runtime = new ElapsedTime();
|
private ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
|
||||||
public double time_for_dist(double speed, double dist){
|
public double time_for_dist(double speed, double dist){
|
||||||
return (double) (dist/speed);
|
return (double) (dist/speed);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() {
|
public void runOpMode() {
|
||||||
lm = hardwareMap.get(DcMotor.class, "blm");
|
lm = hardwareMap.get(DcMotor.class, "blm");
|
||||||
rm = hardwareMap.get(DcMotor.class, "brm");
|
rm = hardwareMap.get(DcMotor.class, "brm");
|
||||||
rm.setDirection(DcMotorSimple.Direction.REVERSE);
|
rm.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
telemetry.addData("Status", "Initialized");
|
telemetry.addData("Status", "Initialized");
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
double tour_par_minute = 300.0;
|
double tour_par_minute = 300.0;
|
||||||
double wheel_width = 9.0e-2;
|
double wheel_width = 9.0e-2;
|
||||||
double wheel_rayon = (wheel_width)/2;
|
double wheel_rayon = (wheel_width)/2;
|
||||||
double wheel_perimeter = wheel_rayon*2*Math.PI;
|
double wheel_perimeter = wheel_rayon*2*Math.PI;
|
||||||
double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second
|
double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second
|
||||||
boolean mode = false;
|
boolean mode = false;
|
||||||
// Wait for the game to start (driver presses PLAY)
|
// Wait for the game to start (driver presses PLAY)
|
||||||
waitForStart();
|
waitForStart();
|
||||||
|
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
if (mode){
|
if (mode){
|
||||||
//mode Elina
|
//mode Elina
|
||||||
while (opModeIsActive() && (runtime.seconds() <= 41e-2*Math.PI/4/speed)) {
|
while (opModeIsActive() && (runtime.seconds() <= 41e-2*Math.PI/4/speed)) {
|
||||||
lm.setPower(1);
|
lm.setPower(1);
|
||||||
rm.setPower(-1);
|
rm.setPower(-1);
|
||||||
telemetry.addData("Leg 1", runtime.seconds());
|
telemetry.addData("Leg 1", runtime.seconds());
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
|
while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
|
||||||
lm.setPower(1);
|
lm.setPower(1);
|
||||||
rm.setPower(1);
|
rm.setPower(1);
|
||||||
telemetry.addData("Leg 2", runtime.seconds());
|
telemetry.addData("Leg 2", runtime.seconds());
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
double[][] operations = {
|
double[][] operations = {
|
||||||
{-1.0,1.0}, // vectors
|
{-1.0,1.0}, // vectors
|
||||||
{1.0,1.0},
|
{1.0,1.0},
|
||||||
{-1.0,1.0},
|
{-1.0,1.0},
|
||||||
{-1.0,-1.0},
|
{-1.0,-1.0},
|
||||||
{1.0,-1.0}
|
{1.0,-1.0}
|
||||||
};
|
};
|
||||||
//mode Aurelien
|
//mode Aurelien
|
||||||
for(int i = 0; i<operations.length; i++){
|
for(int i = 0; i<operations.length; i++){
|
||||||
double[] vec = operations[i];
|
double[] vec = operations[i];
|
||||||
double x = vec[0];
|
double x = vec[0];
|
||||||
double y = vec[1];
|
double y = vec[1];
|
||||||
double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
|
double total_dist = (double) Math.sqrt(Math.pow(y,2)+Math.pow(x,2));
|
||||||
double time = time_for_dist(speed, time);
|
double time = time_for_dist(speed, total_dist);
|
||||||
double a = (-y+x)/Math.pow(2,1/2);
|
double a = (-y+x)/Math.pow(2,1/2);
|
||||||
double b = (-y-x)/Math.pow(2,1/2);
|
double b = (-y-x)/Math.pow(2,1/2);
|
||||||
double vmean = (Math.abs(a)+Math.abs(b))/2;
|
double vmean = (Math.abs(a)+Math.abs(b))/2;
|
||||||
double lmvalue = (a/vmean);
|
double lmvalue = (a/vmean);
|
||||||
double rmvalue = (b/vmean);
|
double rmvalue = (b/vmean);
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
while (opModeIsActive() && (runtime.seconds() <= time)) {
|
while (opModeIsActive() && (runtime.seconds() <= time)) {
|
||||||
lm.setPower(lmvalue);
|
lm.setPower(lmvalue);
|
||||||
rm.setPower(rmvalue);
|
rm.setPower(rmvalue);
|
||||||
telemetry.addData("Runtime Seconds", runtime.seconds());
|
telemetry.addData("Runtime Seconds", runtime.seconds());
|
||||||
telemetry.addData("current_operation",operations[i]);
|
telemetry.addData("current_operation",operations[i]);
|
||||||
telemetry.addData("current_op_id",i);
|
telemetry.addData("current_op_id",i);
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// run until the end of the match (driver presses STOP
|
// run until the end of the match (driver presses STOP
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,7 @@
|
||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
|
||||||
import com.qualcomm.robotcore.robot.Robot;
|
import com.qualcomm.robotcore.robot.Robot;
|
||||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
|
@ -24,155 +25,155 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||||
//test
|
//test
|
||||||
@TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot")
|
@TeleOp(name="WeRobot: FTC2024 Carlike", group="WeRobot")
|
||||||
public class Werobot_FTC2024_carlike extends LinearOpMode {
|
public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||||
private DcMotor rm;
|
private DcMotor rm;
|
||||||
private DcMotor lm;
|
private DcMotor lm;
|
||||||
private DcMotor moissoneuse;
|
private DcMotor moissoneuse;
|
||||||
private IMU imu;
|
private IMU imu;
|
||||||
private double helloexp(double t){
|
private double helloexp(double t){
|
||||||
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
|
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
|
||||||
}
|
}
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() throws InterruptedException {
|
public void runOpMode() throws InterruptedException {
|
||||||
float x;
|
float x;
|
||||||
double y;
|
double y;
|
||||||
double t;
|
double t;
|
||||||
double t2;
|
double t2;
|
||||||
double t3;
|
double t3;
|
||||||
String mode = "normal";
|
String mode = "normal";
|
||||||
boolean already_b = false;
|
boolean already_b = false;
|
||||||
boolean already_a = false;
|
boolean already_a = false;
|
||||||
telemetry.addData("Status", "Initialized");
|
telemetry.addData("Status", "Initialized");
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
lm = hardwareMap.get(DcMotor.class, "blm");
|
lm = hardwareMap.get(DcMotor.class, "blm");
|
||||||
rm = hardwareMap.get(DcMotor.class, "brm");
|
rm = hardwareMap.get(DcMotor.class, "brm");
|
||||||
moissoneuse = hardwareMap.get(DCMotor.class, "flm");
|
moissoneuse = hardwareMap.get(DcMotor.class, "flm");
|
||||||
|
|
||||||
|
|
||||||
imu = hardwareMap.get(IMU.class, "imu");
|
imu = hardwareMap.get(IMU.class, "imu");
|
||||||
imu.initialize(
|
imu.initialize(
|
||||||
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();
|
||||||
rm.setDirection(DcMotorSimple.Direction.REVERSE);
|
rm.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
//telemetry.addData("Mode", "calibrating...");
|
//telemetry.addData("Mode", "calibrating...");
|
||||||
//telemetry.update();
|
//telemetry.update();
|
||||||
|
|
||||||
// make sure the imu gyro is calibrated before continuing.
|
// make sure the imu gyro is calibrated before continuing.
|
||||||
//while (!isStopRequested() && !imu.isGyroCalibrated())
|
//while (!isStopRequested() && !imu.isGyroCalibrated())
|
||||||
//{
|
//{
|
||||||
// sleep(50);
|
// sleep(50);
|
||||||
// idle();
|
// idle();
|
||||||
//}
|
//}
|
||||||
|
|
||||||
telemetry.addData("Mode", "waiting for start");
|
telemetry.addData("Mode", "waiting for start");
|
||||||
//telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
|
//telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
waitForStart();
|
waitForStart();
|
||||||
|
|
||||||
//test
|
//test
|
||||||
while (opModeIsActive()) {
|
while (opModeIsActive()) {
|
||||||
x = gamepad1.left_stick_x;
|
x = gamepad1.left_stick_x;
|
||||||
y = gamepad1.left_stick_y;
|
y = gamepad1.left_stick_y;
|
||||||
t= gamepad1.right_trigger;
|
t= gamepad1.right_trigger;
|
||||||
t2 = helloexp(t);
|
t2 = helloexp(t);
|
||||||
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");
|
||||||
if(gamepad1.a && !already_a){
|
if(gamepad1.a && !already_a){
|
||||||
if(mode=="normal"){
|
if(mode=="normal"){
|
||||||
mode="tank";
|
mode="tank";
|
||||||
}else if(mode=="tank"){
|
}else if(mode=="tank"){
|
||||||
mode = "essaifranck";
|
mode = "essaifranck";
|
||||||
}else if (mode == "essaifranck"){
|
}else if (mode == "essaifranck"){
|
||||||
mode = "elina";
|
mode = "elina";
|
||||||
}else{
|
}else{
|
||||||
mode="normal";
|
mode="normal";
|
||||||
}
|
}
|
||||||
already_a = true;
|
already_a = true;
|
||||||
}
|
}
|
||||||
if(!gamepad1.a && already_a){
|
if(!gamepad1.a && already_a){
|
||||||
already_a = false;
|
already_a = false;
|
||||||
}
|
}
|
||||||
double lpower = 0.0;
|
double lpower = 0.0;
|
||||||
double rpower = 0.0;
|
double rpower = 0.0;
|
||||||
if(mode=="normal"){
|
if(mode=="normal"){
|
||||||
double ysign = y>0?1.0:(y<0?-1.0:0.0);
|
double ysign = y>0?1.0:(y<0?-1.0:0.0);
|
||||||
double xsign = x>0?1.0:(x<0?-1.0:0.0);
|
double xsign = x>0?1.0:(x<0?-1.0:0.0);
|
||||||
lpower = -ysign * t + (xsign-2*x)*t;
|
lpower = -ysign * t + (xsign-2*x)*t;
|
||||||
rpower = ysign * t + (xsign-2*x)*t;
|
rpower = ysign * t + (xsign-2*x)*t;
|
||||||
}
|
}
|
||||||
else if (mode=="tank"){
|
else if (mode=="tank"){
|
||||||
lpower = -y;
|
lpower = -y;
|
||||||
rpower = gamepad1.right_stick_y;
|
rpower = gamepad1.right_stick_y;
|
||||||
}
|
}
|
||||||
else if (mode=="essaifranck"){
|
else if (mode=="essaifranck"){
|
||||||
double a = (-y+x)/Math.pow(2,1/2);
|
double a = (-y+x)/Math.pow(2,1/2);
|
||||||
double b = (-y-x)/Math.pow(2,1/2);
|
double b = (-y-x)/Math.pow(2,1/2);
|
||||||
double vmean = (Math.abs(a)+Math.abs(b))/2;
|
double vmean = (Math.abs(a)+Math.abs(b))/2;
|
||||||
lpower = (a/vmean)*t2;
|
lpower = (a/vmean)*t2;
|
||||||
rpower = (b/vmean)*t2;
|
rpower = (b/vmean)*t2;
|
||||||
}
|
}
|
||||||
else if (mode=="elina"){
|
else if (mode=="elina"){
|
||||||
double a = (-y+x)/Math.pow(2,1/2);
|
double a = (-y+x)/Math.pow(2,1/2);
|
||||||
double b = (-y-x)/Math.pow(2,1/2);
|
double b = (-y-x)/Math.pow(2,1/2);
|
||||||
double vmean = (Math.abs(a)+Math.abs(b))/2;
|
double vmean = (Math.abs(a)+Math.abs(b))/2;
|
||||||
lpower = (a/vmean)*t3;
|
lpower = (a/vmean)*t3;
|
||||||
rpower = (b/vmean)*t3;
|
rpower = (b/vmean)*t3;
|
||||||
}
|
}
|
||||||
if(!(gamepad1.left_bumper)){
|
if(!(gamepad1.left_bumper)){
|
||||||
lpower/=3;
|
lpower/=3;
|
||||||
rpower/=3;
|
rpower/=3;
|
||||||
}
|
}
|
||||||
lm.setPower(lpower);
|
lm.setPower(lpower);
|
||||||
rm.setPower(rpower);
|
rm.setPower(rpower);
|
||||||
|
|
||||||
if(gamepad1.b && !already_b){
|
if(gamepad1.b && !already_b){
|
||||||
already_b = !already_b;
|
already_b = !already_b;
|
||||||
if(moissoneuse.getPowerFloat == (float) 1){
|
if(moissoneuse.getPower() == 1.0){
|
||||||
moissoneuse.setPower(0);
|
moissoneuse.setPower(0);
|
||||||
}else{
|
}else{
|
||||||
moissoneuse.setPower(1);
|
moissoneuse.setPower(1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if(!gamepad1.b && already_b){
|
if(!gamepad1.b && already_b){
|
||||||
already_b = !already_b;
|
already_b = !already_b;
|
||||||
}
|
}
|
||||||
if(gamepad1.a && !already_a){
|
if(gamepad1.a && !already_a){
|
||||||
already_a = !already_a;
|
already_a = !already_a;
|
||||||
if(moissoneuse.getPowerFloat == (float) -1){
|
if(moissoneuse.getPower() == -1.0){
|
||||||
moissoneuse.setPower(0);
|
moissoneuse.setPower(0);
|
||||||
}else{
|
}else{
|
||||||
moissoneuse.setPower(-1);
|
moissoneuse.setPower(-1);
|
||||||
}
|
}
|
||||||
if(!gamepad1.a && already_a){
|
if(!gamepad1.a && already_a){
|
||||||
already_a = !already_a;
|
already_a = !already_a;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
telemetry.addData("x",x);
|
telemetry.addData("x",x);
|
||||||
telemetry.addData("y",y);
|
telemetry.addData("y",y);
|
||||||
telemetry.addData("lpow",lpower);
|
telemetry.addData("lpow",lpower);
|
||||||
telemetry.addData("rpow",rpower);
|
telemetry.addData("rpow",rpower);
|
||||||
telemetry.addData("ltrigg",t);
|
telemetry.addData("ltrigg",t);
|
||||||
telemetry.addData("t2",t2);
|
telemetry.addData("t2",t2);
|
||||||
telemetry.addData("mode",mode);
|
telemetry.addData("mode",mode);
|
||||||
// Create an object to receive the IMU angles
|
// Create an object to receive the IMU angles
|
||||||
YawPitchRollAngles robotOrientation;
|
YawPitchRollAngles robotOrientation;
|
||||||
robotOrientation = imu.getRobotYawPitchRollAngles();
|
robotOrientation = imu.getRobotYawPitchRollAngles();
|
||||||
|
|
||||||
// Now use these simple methods to extract each angle
|
// Now use these simple methods to extract each angle
|
||||||
// (Java type double) from the object you just created:
|
// (Java type double) from the object you just created:
|
||||||
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
|
||||||
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
|
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
|
||||||
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
|
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
|
||||||
telemetry.addData("yaw",Yaw);
|
telemetry.addData("yaw",Yaw);
|
||||||
|
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue