diff --git a/ftc2024-holonom.java b/ftc2024-holonom.java index 171fb06..ad83c78 100644 --- a/ftc2024-holonom.java +++ b/ftc2024-holonom.java @@ -109,8 +109,8 @@ public class Werobot_FTC2024 extends LinearOpMode { 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])+(Math.PI/4); - double diff_angle = joystick_angle - cur_motor_angle; + double cur_motor_angle = Math.atan2(cur_motor[0],cur_motor[1]); + double diff_angle = joystick_angle - cur_motor_angle+(Math.PI/4); if (mode=="normal"){ diff_angle-=Yaw; }