Diff: main.cpp

Revision:

18:1d89ec4148ec

Parent:

17:bafc6a46b3df

Child:

19:0c1c3b2e009f

--- a/main.cpp	Thu Mar 05 11:22:58 2020 +0000
+++ b/main.cpp	Thu Mar 05 12:46:31 2020 +0000
@@ -97,9 +97,9 @@
 int plot[5][2]= {
     {0,0}
     ,{0,10000}
-    ,{2500,1000}
-    ,{2500,0}
-    ,{0,0}
+    ,{5000,10000}
+    ,{5000,0}
+    ,{2000,5000}
 };
 
 //出力を計算
@@ -108,7 +108,7 @@
 class WheelOmega
 {
 public:
-    WheelOmega(): max_(0),vx_(0),vy_(0)
+    WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0)
     {
         for(int i=0; i<4; i++) {
             omega[i]=0;
@@ -122,48 +122,35 @@
             max_ = -1 * max;
         }
     }
-    void setVxy(double vx,double vy)
+    void setVxy(double vx,double vy,double theta)
     {
         vx_=vx;
         vy_=vy;
+        theta_=theta;
     }
     void calOmega()
     {
-        omega[0]=max_*vx_/sqrt(2.0)-max_*vy_/sqrt(2.0);
-        omega[1]=-max_*vx_/sqrt(2.0)-max_*vy_/sqrt(2.0);
-        omega[2]=-max_*vx_/sqrt(2.0)+max_*vy_/sqrt(2.0);
-        omega[3]=max_*vx_/sqrt(2.0)+max_*vy_/sqrt(2.0);
+        omega[0]=max_*vx_/sqrt(2.0)-max_*vy_/sqrt(2.0)-theta*max_*0.005;
+        omega[1]=-max_*vx_/sqrt(2.0)-max_*vy_/sqrt(2.0)-theta*max_*0.005;
+        omega[2]=-max_*vx_/sqrt(2.0)+max_*vy_/sqrt(2.0)-theta*max_*0.005;
+        omega[3]=max_*vx_/sqrt(2.0)+max_*vy_/sqrt(2.0)-theta*max_*0.005;
     };
     double getOmega(int i)
     {
         return omega[i];
     }
 private:
-    double max_,vx_,vy_;
+    double max_,vx_,vy_,theta_;
     double omega[4];
 };
 
 WheelOmega omega;
-//出力
-int a=0;
-void motorOut()
-{
-    for(int i=0; i<4; i++) {
-        motor[i].Sc(omega.getOmega(i));
-        if(a%10==1) {
-            for(int i=0; i<4; i++) {
-                //printf("%d %.2f /",i,omega.getOmega(i));
-            }
-            //printf("\r\n");
-        }
-        a++;
-    }
-}
+//パラメタ処理
 double pControl(int dx_,int dy_,double time_)
 {
     double distance,z,zMax;
     distance = sqrt((double)dx_*dx_+dy_*dy_);
-    z=0.005*distance;
+    z=0.004*distance;
     zMax=10;
     if(z>zMax) {
         z=zMax;
@@ -174,10 +161,22 @@
     return z;
 }
 
+//出力
+int a=0;
+void motorOut()
+{
+    for(int i=0; i<4; i++) {
+        motor[i].Sc(omega.getOmega(i));
+    }
+}
+
 int main()
 {
     gyro.initialize();    //main関数の最初に一度だけ実行
     gyro.acc_offset();
+double angle;
+angle=gyro.getAngle();
+    double z;
     printf("start\r\n");
     motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
     motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
@@ -210,12 +209,11 @@
     time.start();
     while(1) {
         //自己位置取得
-        theta=gyro.getAngle();    //角度の値を受け取る
+        theta=gyro.getAngle()-angle;    //角度の値を受け取る
         location.calXY();
 
         x=location.getX();
         y=location.getY();
-        double z;
         printf("X=%d,Y=%d theta=%5.3f z=%5.3f %f \r\n",x,y,theta,z,time.read());
 
         //目的地決定(syuusoku check)
@@ -226,29 +224,30 @@
         dy=aimY-y;
         vx=dx/sqrt((double)dx*dx+dy*dy);
         vy=dy/sqrt((double)dx*dx+dy*dy);
-
         //四輪の出力計算
         z=pControl(dx,dy,time.read());
         omega.setOmega(z);
-        omega.setVxy(vx,vy);
+        omega.setVxy(vx,vy,theta);
         omega.calOmega();
-
+        //ゴール判定
         if(dx<500 && dx>-500 && dy<500 && dy>-500) {
             n++;
-            printf("reach");
+            printf("reach%d\r\n",n);
             ticker.detach();
-            for(int j=0; j<4; j++) {
+            /*for(int j=0; j<4; j++) {
                 motor[j].stop();
             }
-            wait(1);
+            wait(1);*/
             time.reset();
+            ticker.attach(motorOut,0.05);
         }
 
-        if(n>4) {
+        if(n>=5) {
             for(int j=0; j<4; j++) {
                 motor[j].Sc(0);
             }
             while(1) {
+                
                 printf("fin");
                 ticker.detach();
             }