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

feat:ftc2024-holonom.java

Browse source
This commit is contained in:
GZod01 2024-01-07 16:53:04 +01:00
parent 2548c8a651
commit be9c1f6e74
1 changed files with 20 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
ftc2024-holonom.java
View file

@ -18,6 +18,7 @@ public class Werobot_FTC2024 extends LinearOpMode {
private DcMotor frm;
private DcMotor brm;
private DcMotor blm;
private IMU imu;
@Override
public void runOpMode() throws InterruptedException {
float x;
@ -37,11 +38,29 @@ public class Werobot_FTC2024 extends LinearOpMode {
frm = hardwareMap.get(DcMotor.class, "frm");
blm = hardwareMap.get(DcMotor.class, "blm");
brm = hardwareMap.get(DcMotor.class, "brm");
imu = hardwareMap.get(IMU.class, "imu");
imu.initialize(
new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
)
)
);
imu.resetYaw();
waitForStart();
while (opModeIsActive()) {
telemetry.addData("Status", "Running");
YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles();
// Now use these simple methods to extract each angle
// (Java type double) from the object you just created:
double Yaw = robotOrientation.getYaw(AngleUnit.RADIAN);
double Pitch = robotOrientation.getPitch(AngleUnit.RADIAN);
double Roll = robotOrientation.getRoll(AngleUnit.RADIAN);
double[] motors_values = new double[4];
telemetry.addData("left_stick_x", gamepad1.left_stick_x);
telemetry.addData("left_stick_y", gamepad1.left_stick_y);
@ -61,6 +80,7 @@ public class Werobot_FTC2024 extends LinearOpMode {
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;
diff_angle+=Yaw;
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);