Diff: main.cpp

Revision:

7:4e77a1ae70d1

Parent:

6:6ce8adb328fa

Child:

8:89827b0d8a93

diff -r 6ce8adb328fa -r 4e77a1ae70d1 main.cpp
--- a/main.cpp	Tue Dec 10 07:29:41 2019 +0000
+++ b/main.cpp	Thu Dec 12 08:54:28 2019 +0000
@@ -3,10 +3,15 @@
 #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, 115200);
-SerialMultiByte arduinocontroller(PC_10,PC_11);
+SerialMultiByte arduino(PC_10,PC_11);
 
 PwmOut beep(PA_0);
 DigitalOut led1(PA_11);
@@ -19,12 +24,11 @@
     };
     
 const double PII = 3.14159265358979;
-OmniWheel omni(4);
+OmniWheel omni(3);
 
 void setup(){
-        arduinocontroller.setHeaders(127,127);
-        arduinocontroller.startReceive(13);
-        
+        arduino.setHeaders(127,127);
+        arduino.startReceive(5);
         omni.wheel[0].setRadian(PII/2.0);
         omni.wheel[1].setRadian(7.0/6.0*PII);
         omni.wheel[2].setRadian(11.0/6.0*PII);
@@ -67,43 +71,66 @@
 
 int main() {
     setup();
-    float x, y;
-    
-    uint8_t button[13];
+    float x, y,now_angle,angle_deta;
+    jy.calibrateAll(50);
+    uint8_t weight[5];
+    angle.setInputLimits(-180.0,180.0);
+    angle.setOutputLimits(-1,1);
+    angle.setBias(0);
+    angle.setMode(1);
+    angle.setSetPoint(0);
     while (true) {
         led2=1;
-        arduinocontroller.getData(button);
-        for (uint8_t i = 0; i < 13; i++) {
-            pc.printf("%d",button[i]);
+        arduino.getData(weight);
+        for (uint8_t i = 0; i < 4; i++) {
+            pc.printf("%d",weight[i]);
             pc.printf("\t");
         }
-        
-        if(button[0]==1){
+
+        y = weight[0] + weight[2] - weight[1] - weight[3];
+        x = weight[0] + weight[1] - weight[2] - weight[3];
+        if(y < -50){
             y = -0.5;
+        }else if(y > 50){
+            y = 0.5;
+        }else{
+            y = 0;
         }
-        
-        if(button[1]==1){
-            y = 0.5;
+        if(x < -50){
+            x = -0.5;
+        }else if(x > 50){
+            x = 0.5;
+        }else{
+            x = 0;
         }
+        now_angle = jy.getZaxisAngle();
         
-        if(button[2]==1){
-            x = 0.5;
+        if(now_angle>180 && now_angle<360){
+            now_angle = now_angle-360;
         }
-        
-        if(button[3]==1){
-            x = -0.5;
-        }
-         
+        angle.setProcessValue(now_angle);
         omni.computeXY(
             x,
             y,
             0,
             0,
-            0
+            angle.compute()
+            
         );
         for(int i=0; i<3; i++){
             motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
         }
-        pc.printf("X = <%f>, Y = <%f>\r\n",x,y);
+        if(weight[0]>40 && weight[3]>40){
+            for(int i=0; i<3; i++){
+                motor[i].setMotorSpeed(0.2);
+            }
+        }
+        
+        if(weight[1]>40 && weight[2]>40){
+            for(int i=0; i<3; i++){
+                motor[i].setMotorSpeed(-0.2);
+            }
+        }
+        pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
     }
 }
\ No newline at end of file