Diff: main.cpp

Revision:

1:5c0b7355adcd

Parent:

0:46c70e5b719d

--- a/main.cpp	Thu May 16 07:41:44 2019 +0000
+++ b/main.cpp	Thu May 16 08:43:30 2019 +0000
@@ -2,35 +2,41 @@
 #include "controller.h"
 #include "ikarashiMDC.h"
 #include "R1370.h"
-#include "wheel_an.h"
 #define PI 3.141593
+#include"omni_wheel.h"
+#include"PID.h"
 
+PID pid1(3.0,3.0,0.000005,0.01);
+OmniWheel omni(3);
 Controller pad(PC_10, PC_11, 208);
-R1370 R1370(D10,D2);
+R1370 R1370(PB_6,PA_10);
 Serial pc(USBTX, USBRX, 115200);
 Serial serial(PC_6, PC_7, 115200);
-DigitalOut serialcontrol(PD_2);
+DigitalOut serialcontrol(D10);
 
-ikarashiMDC ikarashi[]
+ikarashiMDC motor[]=
 {
     ikarashiMDC(&serialcontrol,2,0,SM,&serial),
     ikarashiMDC(&serialcontrol,2,1,SM,&serial),
     ikarashiMDC(&serialcontrol,2,2,SM,&serial)
 };
 
-Wheelan wheel[]
-{
-    Wheelan(PI * 1 / 6),
-    Wheelan(PI * 5 / 6),
-    Wheelan(PI * 9 / 6)
-};
-
 int main()
 {
-    ikarashi[0].braking = true;
+    pid1.setInputLimits(-90,90);
+    pid1.setOutputLimits(-0.5,0.5);
+    pid1.setBias(0);
+    pid1.setMode(1);
+    pid1.setSetPoint(0);
+    omni.wheel[0].setRadian(PI * 1.0 / 6.0);
+    omni.wheel[1].setRadian(PI * 5.0 / 6.0);
+    omni.wheel[2].setRadian(PI * 9.0 / 6.0);
+  
+    motor[0].braking = true;
     int b[11];
     double rad[2],dis[2];
     double value[3];
+    pc.printf("saaa");
     while(1){
         if(pad.receiveState()){
             for(int i = 0; i < 13; i++){
@@ -41,36 +47,22 @@
                 dis[i] = pad.getNorm(i);
                 rad[i] = PI - rad[i];
             }
+            double X = dis[0]*cos(rad[0]);
+            double Y = dis[0]*sin(rad[0]);
             R1370.update();
-            pc.printf("degree : %.3lf    ", R1370.getAngle());
-
-            for (int i = 0; i < 3; ++i)
-            {
-                value[i] = wheel[i].an_uc(rad[1], dis[1]);
-            }
-
-            if(b[7] == 1 && b[9] == 0) {
-                value[0] -= 0.3;
-                value[1] -= 0.3;
-                value[2] -= 0.3;
-            } else if (b[9] == 1 && b[7] == 0) {
-                value[0] += 0.3;
-                value[1] += 0.3;
-                value[2] += 0.3;
-            }
-
-            for (int i = 0; i < 3; ++i)
-            {
-                pc.printf("%lf ", value[i]);
-                ikarashi[i].setSpeed(value[i]);
-            }
-            pc.printf("\n\r");
-
+            double angle = R1370.getAngle();
+            pid1.setProcessValue(angle);
+            double spin_power = pid1.compute();
+            omni.computeXY(X,Y,-spin_power);
+            pc.printf("%.3f||%.3f||%.3f||%.3f\n\r",X,Y,angle,spin_power);
+            for(int i = 0; i < 3; i++)
+                motor[i].setSpeed(omni.wheel[i]);
+            
         }else{
             pc.printf("error\n\r");
             for (int i = 0; i < 3; i++)
             {
-                ikarashi[i].setSpeed(0);
+                motor[i].setSpeed(0);
             }
         }
     }