Diff: main.cpp

Revision:

13:0810457baab8

Parent:

12:8d5e9965e8f2

Child:

14:03e27cf2c6b3

--- a/main.cpp	Fri Jan 24 08:58:19 2020 +0000
+++ b/main.cpp	Fri Jan 24 09:32:07 2020 +0000
@@ -1,7 +1,6 @@
 #define BEAT 140
 #include "mbed.h"
 #include "math.h"
-#include "omni_wheel.h"
 #include "motorsmlap.h"
 #include "SerialMultiByte.h"
 
@@ -64,6 +63,7 @@
 int main() {
     setup();
     float x, y;
+    double speed[3]={0};
     while (true) {
         led2=1;
         int c = pc.getc();
@@ -83,17 +83,12 @@
             x=0;
             y=0;
             }
-        omni.computeXY(
-            x,
-            y,
-            0,
-            0,
-            0
-            
-        );
+        
         for(int i=0; i<3; i++){
-            motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
+            speed[i] = sin(atan2(y,x) - PI * (i*4 + 3) / 6);
+            motor[i].setMotorSpeed(speed[i]*hypot(x,y));
+            //pc.printf("%f ",speed[i]);
         }
-        pc.printf("X = <%f>,Y = <%f>\r\n",x,y);
+        //pc.printf("X = <%f>,Y = <%f>\r\n",x,y);
     }
 }
\ No newline at end of file