Diff: main.cpp

Revision:

16:7ce4ab00621a

Parent:

15:e26bad257626

Child:

17:bafc6a46b3df

--- a/main.cpp	Thu Mar 05 08:50:38 2020 +0000
+++ b/main.cpp	Thu Mar 05 09:56:08 2020 +0000
@@ -1,3 +1,4 @@
+
 #include "mbed.h"
 #include "EC.h"
 #include "SpeedController.h"
@@ -154,8 +155,20 @@
         a++;
     }
 }
-
-double z=1;
+double pControl(int dx_,int dy_,double time_)
+{
+    double distance,z,zMax;
+    distance = sqrt((double)dx_*dx_+dy_*dy_);
+    z=0.003*distance;
+    zMax=20;
+    if(z>zMax) {
+        z=zMax;
+    }
+    if(time_<2) {
+        z=z*(time_/2);
+    }
+    return z;
+}
 
 int main()
 {
@@ -189,6 +202,8 @@
     int n=1,dx,dy,aimX,aimY;
     double vx,vy;
     Location location;
+    Timer time;
+    time.start();
     while(1) {
         //自己位置取得
         theta=gyro.getAngle();    //角度の値を受け取る
@@ -196,7 +211,8 @@
 
         x=location.getX();
         y=location.getY();
-        printf("X=%d,Y=%d theta=%5.3f z=%5.3f \r\n",x,y,theta,z);
+        double z;
+        printf("X=%d,Y=%d theta=%5.3f z=%5.3f %f \r\n",x,y,theta,z,time.read());
 
         //目的地決定(syuusoku check)
         aimX = plot[n][0];
@@ -208,37 +224,23 @@
         vy=dy/sqrt((double)dx*dx+dy*dy);
 
         //四輪の出力計算
+        z=pControl(dx,dy,time.read());
         omega.setOmega(z);
         omega.setVxy(vx,vy);
         omega.calOmega();
 
-        if(dx<300 &&dx>-300) {
-            if(dy<300 && dy>-300) {
-                if(z>5) {
-                    z=z-0.1;
-                }
-                if(dx<50 && dx>-50) {
-                    if(dy<50 && dy>-50) {
-                        n++;
-                        printf("reached Location %d\r\n",n);
-                        ticker.detach();
-                        for(int j=0; j<4; j++) {
-                            motor[j].Sc(0);
-                        }
-                        wait(2);
-                        ticker.attach(motorOut,0.05);
-                    }
-                }
+        if(dx<50 && dx>-50 && dy<50 && dy>-50) {
+            n++;
+            printf("reach");
+            ticker.detach();
+            for(int j=0; j<4; j++) {
+                motor[j].stop();
             }
-
+            time.reset();
+            wait(1);
+            time.start();
         }
-        if(dx>300 || dx<-300) {
-            if(dy>300 || dy<-300) {
-                if(z<10) {
-                    z=z+0.1;
-                }
-            }
-        }
+
         if(n>4) {
             for(int j=0; j<4; j++) {
                 motor[j].Sc(0);