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 17:26:01 +01:00
parent a6db57fcdb
commit 35a29749da
1 changed files with 2 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
ftc2024-holonom.java
View file

@ -91,7 +91,7 @@ public class Werobot_FTC2024 extends LinearOpMode {
double joystick_angle = Math.atan2(joystick_vector[0],joystick_vector[1]); double joystick_angle = Math.atan2(joystick_vector[0],joystick_vector[1]);
double cur_motor_angle = Math.atan2(cur_motor[0],cur_motor[1]); double cur_motor_angle = Math.atan2(cur_motor[0],cur_motor[1]);
double diff_angle = joystick_angle - cur_motor_angle; 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] = (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]+gamepad1.right_stick_x)/(Math.abs(gamepad1.right_stick_x)+1);
motors_values[i]= motors_values[i]/Math.sqrt(2); motors_values[i]= motors_values[i]/Math.sqrt(2);
@ -107,6 +107,7 @@ public class Werobot_FTC2024 extends LinearOpMode {
frm.setPower(motors_values[1]); frm.setPower(motors_values[1]);
blm.setPower(motors_values[2]); blm.setPower(motors_values[2]);
brm.setPower(motors_values[3]); brm.setPower(motors_values[3]);
telemetry.addData("yaw",Yaw);
telemetry.update(); telemetry.update();
} }
} }