From 35a29749da419a99467c8070046819061df6c5c8 Mon Sep 17 00:00:00 2001 From: GZod01 Date: Sun, 7 Jan 2024 17:26:01 +0100 Subject: [PATCH] feat:ftc2024-holonom.java --- ftc2024-holonom.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ftc2024-holonom.java b/ftc2024-holonom.java index 7110d9f..f90b4a1 100644 --- a/ftc2024-holonom.java +++ b/ftc2024-holonom.java @@ -91,7 +91,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; + diff_angle-=Yaw-Math.PI; 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); @@ -107,6 +107,7 @@ public class Werobot_FTC2024 extends LinearOpMode { frm.setPower(motors_values[1]); blm.setPower(motors_values[2]); brm.setPower(motors_values[3]); + telemetry.addData("yaw",Yaw); telemetry.update(); } }