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

Update ftc2024-holonom.java

Browse source
This commit is contained in:
GZod01 2024-01-21 14:49:46 +01:00 committed by GitHub
parent 25e098b9be
commit 70ed024134
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
1 changed files with 2 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
ftc2024-holonom.java
View file

@ -109,8 +109,8 @@ public class Werobot_FTC2024 extends LinearOpMode {
int[] cur_motor = motors_angles[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 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 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 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+(Math.PI/4);
if (mode=="normal"){ if (mode=="normal"){
diff_angle-=Yaw; diff_angle-=Yaw;
} }