Diff: main.cpp

Revision:

22:5c78d75fb2f2

Parent:

21:46b85fee5e88

diff -r 46b85fee5e88 -r 5c78d75fb2f2 main.cpp
--- a/main.cpp	Mon Mar 09 09:16:48 2020 +0000
+++ b/main.cpp	Mon Mar 09 11:56:16 2020 +0000
@@ -84,7 +84,7 @@
 };
 
 double aimTheta[5]= {//目標角度を指定
-    0,-90,-180,-90,0
+    0,-30,-180,-90,0
 };
 
 double zMin[5]= { //速度の最少を指定
@@ -117,10 +117,11 @@
     }
     void calOmega()
     {
-        omega[0]=max_*vx_*cos(45+theta)-max_*vy_*cos(45-theta)+theta_*k_;
-        omega[1]=-max_*vx_*cos(45-theta)-max_*vy_*cos(45+theta)+theta_*k_;
-        omega[2]=-max_*vx_*cos(45+theta)+max_*vy_*cos(45-theta)+theta_*k_;
-        omega[3]=max_*vx_*cos(45-theta)+max_*vy_*cos(45+theta)+theta_*k_;
+        double theta_rad=45/180*3.14;
+        omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
+        omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
+        omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
+        omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
     };
     double getOmega(int i)
     {
@@ -140,7 +141,7 @@
     diftime_ = newtime - oldtime;
     oldtime= newtime;
     z=0.004*distance_ - 0.004*(olddistance-distance_)/diftime_;
-    zMax=15;
+    zMax=10;
     if(z>zMax) {
         z=zMax;
     }
@@ -196,7 +197,7 @@
     }
 
     int n=1,dx,dy,aimX,aimY;
-    double vx,vy,newtime,distance;
+    double vx_,vy_,vx,vy,newtime,distance;
     Location location;
     Timer time;
     time.start();
@@ -207,7 +208,7 @@
 
         x=location.getX();
         y=location.getY();
-        printf("X=%d,Y=%d theta=%5.3f z=%5.3f %f %f\r\n",x,y,theta,z,gyro.getAngle(),time.read());
+        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];
@@ -215,29 +216,22 @@
         //出力を計算(kitai xy);
         dx=aimX-x;
         dy=aimY-y;
-        vx=dx/sqrt((double)dx*dx+dy*dy);
-        vy=dy/sqrt((double)dx*dx+dy*dy);
+        vx_=dx/sqrt((double)dx*dx+dy*dy);
+        vy_=dy/sqrt((double)dx*dx+dy*dy);
+        vx=vx_*cos(theta/360*3.14)+vy_*sin(theta/180*3.14);
+        vy=vx_*sin(theta/360*3.14)+vy_*cos(theta/180*3.14);
         //四輪の出力計算
         newtime=time.read();
         distance = sqrt((float)dx*dx+dy*dy);
         z=pControl(distance,zMin[n],newtime);
-        omega.setOmega(z,0.02);
+        omega.setOmega(z,0.05);
         omega.setVxy(vx,vy,aimTheta[n]);
         omega.calOmega();
         //ゴール判定
-
-        int d;
         if(distance<400) {
-            z=0;
-            while(1) {
-                d=aimTheta[n]-theta;
-                if(d>-10 && d<10) {
-                    break;
-                }
-                time.reset();
-            }
             n++;
             printf("reach%d\r\n",n);
+            time.reset();
         }
 
         if(n>=5) {