Diff: main.cpp

Revision:

1:ea880e226e5a

Parent:

0:a1238c4cd105

Child:

2:612efbe94f42

Child:

5:e189d55ef292

--- a/main.cpp	Wed Jan 19 12:43:26 2022 +0000
+++ b/main.cpp	Thu Jan 20 07:27:53 2022 +0000
@@ -40,10 +40,11 @@
         timer = Time.read_us();
         bno.setmode(OPERATION_MODE_IMUPLUS);
         bno.get_angles();
-        theta = (360 - (double)bno.euler.yaw) / 180 * PI;
-    
+        
+        theta = bno.euler.yaw * (PI / 180);
+        
         if(theta > PI){
-            theta = theta - 2 * PI;
+            theta = -(2 * PI - theta);
         }
         
         DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
@@ -60,7 +61,7 @@
         pid_3.pass_val(data_3.get(),TG.obt_target3());
         pid_4.pass_val(data_4.get(),TG.obt_target4());
         
-        pid_1.wheel_ctl(PB_9,PB_8);
+        pid_1.wheel_ctl(PC_9,PC_8);
         pid_2.wheel_ctl(PB_14,PB_13);
         pid_3.wheel_ctl(PB_5,PB_4);
         pid_4.wheel_ctl(PB_7,PB_6);