Diff: main.cpp

Revision:

12:8d5e9965e8f2

Parent:

11:85568f4e4eaf

Child:

13:0810457baab8

--- a/main.cpp	Fri Jan 24 07:32:57 2020 +0000
+++ b/main.cpp	Fri Jan 24 08:58:19 2020 +0000
@@ -3,14 +3,9 @@
 #include "math.h"
 #include "omni_wheel.h"
 #include "motorsmlap.h"
-#include "jy901.h"
 #include "SerialMultiByte.h"
-#include "PID.h"
 
-JY901 jy(PB_9, PB_8); //sda, scl
-
-PID angle(2.0,0,0,0.01);
-Serial pc(USBTX, USBRX, 9600);
+Serial pc(USBTX, USBRX, 115200);
 
 PwmOut beep(PA_0);
 DigitalOut led1(PA_11);
@@ -68,13 +63,7 @@
 
 int main() {
     setup();
-    float x, y,now_angle;
-    jy.calibrateAll(50);
-    angle.setInputLimits(-180.0,180.0);
-    angle.setOutputLimits(-1,1);
-    angle.setBias(0);
-    angle.setMode(1);
-    angle.setSetPoint(0);
+    float x, y;
     while (true) {
         led2=1;
         int c = pc.getc();
@@ -94,23 +83,17 @@
             x=0;
             y=0;
             }
-        now_angle = jy.getZaxisAngle();
-        
-        if(now_angle>180 && now_angle<360){
-            now_angle = now_angle-360;
-        }
-        angle.setProcessValue(now_angle);
         omni.computeXY(
             x,
             y,
             0,
             0,
-            angle.compute()
+            0
             
         );
         for(int i=0; i<3; i++){
             motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
         }
-        pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
+        pc.printf("X = <%f>,Y = <%f>\r\n",x,y);
     }
 }
\ No newline at end of file