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

Ajouter ftc2024-holonom.java

Browse source
This commit is contained in:
Gzod01 2023-12-17 15:35:44 +00:00
parent b994dbde6a
commit 7e2c5cfdaa
1 changed files with 82 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

82
ftc2024-holonom.java Normal file
View file

@ -0,0 +1,82 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.robot.Robot;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.robot.Robot;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name="WeRobot: FTC2024", group="WeRobot")
public class Werobot_FTC2024 extends LinearOpMode {
private DcMotor flm;
private DcMotor frm;
private DcMotor brm;
private DcMotor blm;
@Override
public void runOpMode() throws InterruptedException {
float x;
double y;
int[] fr_angle = {1,1};
int[] fl_angle = {1,-1};
int[] bl_angle = {-1,-1};
int[] br_angle = {-1, 1};
int[][] motors_angles = {fl_angle,
fr_angle,
bl_angle,
br_angle
};
telemetry.addData("Status", "Initialized");
telemetry.update();
flm = hardwareMap.get(DcMotor.class, "flm");
frm = hardwareMap.get(DcMotor.class, "frm");
blm = hardwareMap.get(DcMotor.class, "blm");
brm = hardwareMap.get(DcMotor.class, "brm");
waitForStart();
while (opModeIsActive()) {
telemetry.addData("Status", "Running");
double[] motors_values = new double[4];
telemetry.addData("left_stick_x", gamepad1.left_stick_x);
telemetry.addData("left_stick_y", gamepad1.left_stick_y);
double[] joystick_vector = {(double) gamepad1.left_stick_x,gamepad1.left_stick_y};
double joystick_norm = Math.pow(
(
(
Math.pow(joystick_vector[0],2)
)+(
Math.pow(joystick_vector[1],2)
)
),(1.0/2.0)
);
for(int i = 0; i<motors_angles.length; i++){
int[] cur_motor = motors_angles[i];
double cur_motor_norm = Math.pow(((Math.pow(cur_motor[0],2))+(Math.pow(cur_motor[1],2))),(1.0/2.0));
double joystick_angle = Math.atan2(joystick_vector[0],joystick_vector[1]);
double cur_motor_angle = Math.atan2(cur_motor[0],cur_motor[1]);
double diff_angle = joystick_angle - cur_motor_angle;
motors_values[i] = (cur_motor_norm*joystick_norm*Math.cos(diff_angle));
motors_values[i] = (motors_values[i]+gamepad1.right_stick_x)/(Math.abs(gamepad1.right_stick_x)+1);
motors_values[i]= motors_values[i]/Math.sqrt(2);
if(!(gamepad1.left_trigger > 0.3)){
motors_values[i]= (motors_values[i])/(3*Math.sqrt(2));
}
telemetry.addData("joystick_norm of "+i+" ",joystick_norm);
telemetry.addData("motors_values["+i+"]",motors_values[i]);
}
flm.setPower(motors_values[0]);
frm.setPower(motors_values[1]);
blm.setPower(motors_values[2]);
brm.setPower(motors_values[3]);
telemetry.update();
}
}
}