Diff: main.cpp

Revision:

20:acb13e232678

Parent:

19:0c1c3b2e009f

Child:

21:46b85fee5e88

--- a/main.cpp	Thu Mar 05 13:09:29 2020 +0000
+++ b/main.cpp	Mon Mar 09 08:35:41 2020 +0000
@@ -5,20 +5,11 @@
 #include "math.h"
 #include "R1370P.h"
 
-//PwmOut motor_1F(PA_5);//1Forward Right motor Forward
-//PwmOut motor_1B(PC_7);//Forward Right motor Back
-//PwmOut motor_2F(PC_9);//2Forward Left motor Forward
-//PwmOut motor_2B(PA_1);//Forward Left motor Back
-//PwmOut motor_3F(PA_10);//3Back Right motor Forward
-//PwmOut motor_3B(PB_4);//Back Right motor Back
-//PwmOut motor_4F(PA_9);//4Back Left motor Forward
-//PwmOut motor_4B(PA_7);//Back Left motor Back
-
 Ec2multi ec[]= {Ec2multi(PC_5,PB_2,RESOLUTION),
                 Ec2multi(PA_11,PB_1,RESOLUTION),
                 Ec2multi(PB_12,PB_15,RESOLUTION),
                 Ec2multi(PC_4,PB_14,RESOLUTION)
-               };  //1逓倍用class
+               };  //2逓倍用class
 
 Ec2multi ecXY[]= {Ec2multi(PC_6,PB_8,RESOLUTION),
                   Ec2multi(PC_8,PB_9,RESOLUTION)
@@ -30,21 +21,12 @@
                        SpeedControl(PA_9,PA_7,50,ec[3])
                       };
 
-//R1370P gyro(PA_11,PA_12);
-
 DigitalIn button(USER_BUTTON);
 Serial pc(USBTX, USBRX); // tx, rx
 R1370P gyro(PC_10,PC_11);   // tx, rx
 
 Ticker ticker;
 
-void calOmega()
-{
-    for(int i=0; i<4; i++) {
-        ec[i].calOmega();
-    }
-}
-
 //自己位置取得
 double theta=0;
 class Location
@@ -95,12 +77,20 @@
 //目的地決定
 int plot[5][2]= {
     {0,0}
-    ,{0,10000}
-    ,{5000,10000}
+    ,{0,5000}
+    ,{5000,5000}
     ,{5000,0}
     ,{2000,5000}
 };
 
+double aimTheta[5]= {//目標角度を指定
+    0,-90,-180,-90,0
+};
+
+double zMin[5]= { //速度の最少を指定
+    0,2,2,2,0
+};
+
 //出力を計算
 int x,y;
 
@@ -113,53 +103,54 @@
             omega[i]=0;
         }
     }
-    void setOmega(int max)
+    void setOmega(double max,double k)
     {
-        if(max >= 0) {
-            max_=max;
-        } else {
-            max_ = -1 * max;
-        }
+        max_=max;
+        k_=k;
+
     }
-    void setVxy(double vx,double vy,double theta)
+    void setVxy(double vx,double vy,double aimtheta_)
     {
         vx_=vx;
         vy_=vy;
-        theta_=theta;
+        theta_=aimtheta_ - theta;
     }
     void calOmega()
     {
-        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;
+        omega[0]=max_*vx_/sqrt(2.0)-max_*vy_/sqrt(2.0)+theta_*k_;
+        omega[1]=-max_*vx_/sqrt(2.0)-max_*vy_/sqrt(2.0)+theta_*k_;
+        omega[2]=-max_*vx_/sqrt(2.0)+max_*vy_/sqrt(2.0)+theta_*k_;
+        omega[3]=max_*vx_/sqrt(2.0)+max_*vy_/sqrt(2.0)+theta_*k_;
     };
     double getOmega(int i)
     {
         return omega[i];
     }
 private:
-    double max_,vx_,vy_,theta_;
+    double max_,vx_,vy_,theta_,k_;
     double omega[4];
 };
 
 WheelOmega omega;
 //パラメタ処理
-double pControl(int dx_,int dy_,double time_,double diftime_)
+double pControl(double distance_,double zMin,double newtime)
 {
-    double distance,z,zMax,olddistance;
-    
-    //double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_);
-    distance = sqrt((double)dx_*dx_+dy_*dy_);
-    z=0.004*distance - 0.004*(olddistance-distance)/diftime_;
-    zMax=10;
+    double z,zMax,olddistance,oldtime;
+    double diftime_;
+    diftime_ = newtime - oldtime;
+    oldtime= newtime;
+    z=0.004*distance_ - 0.004*(olddistance-distance_)/diftime_;
+    zMax=15;
     if(z>zMax) {
         z=zMax;
     }
-    if(time_<1) {
-        z=z*time_;
+    if(z<zMin) {
+        z=zMin;
     }
-    olddistance = distance;
+    if(newtime<1) {
+        z=z*newtime;
+    }
+    olddistance = distance_;
     return z;
 }
 
@@ -176,8 +167,8 @@
 {
     gyro.initialize();    //main関数の最初に一度だけ実行
     gyro.acc_offset();
-double angle;
-angle=gyro.getAngle();
+    double angle;
+    angle=gyro.getAngle();
     double z;
     printf("start\r\n");
     motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
@@ -190,10 +181,10 @@
     motor[2].setDutyLimit(0.5);
     motor[3].setDutyLimit(0.5);
 
-    motor[0].setPDparam( 0.02000, 0.0005 );
-    motor[1].setPDparam( 0.02000, 0.0005 );
-    motor[2].setPDparam( 0.02000, 0.0005 );
-    motor[3].setPDparam( 0.02000, 0.0005);
+    motor[0].setPDparam( 0.1790, 0.00560);
+    motor[1].setPDparam( 0.1705, 0.00620);
+    motor[2].setPDparam( 0.1790, 0.00620);
+    motor[3].setPDparam( 0.1680, 0.00560);
 
     while(1) {
         printf("waiting\r\n");
@@ -205,7 +196,7 @@
     }
 
     int n=1,dx,dy,aimX,aimY;
-    double vx,vy,oldtime,newtime,diftime;
+    double vx,vy,newtime,distance;
     Location location;
     Timer time;
     time.start();
@@ -216,7 +207,7 @@
 
         x=location.getX();
         y=location.getY();
-        printf("X=%d,Y=%d theta=%5.3f z=%5.3f %f \r\n",x,y,theta,z,time.read());
+        printf("X=%d,Y=%d theta=%5.3f z=%5.3f %f %f\r\n",x,y,theta,z,gyro.getAngle(),time.read());
 
         //目的地決定(syuusoku check)
         aimX = plot[n][0];
@@ -227,22 +218,26 @@
         vx=dx/sqrt((double)dx*dx+dy*dy);
         vy=dy/sqrt((double)dx*dx+dy*dy);
         //四輪の出力計算
-        diftime = newtime - oldtime;
-        z=pControl(dx,dy,time.read(),diftime);
-        omega.setOmega(z);
-        omega.setVxy(vx,vy,theta);
+        newtime=time.read();
+        distance = sqrt((float)dx*dx+dy*dy);
+        z=pControl(distance,zMin[n],newtime);
+        omega.setOmega(z,0.02);
+        omega.setVxy(vx,vy,aimTheta[n]);
         omega.calOmega();
         //ゴール判定
-        if(dx<500 && dx>-500 && dy<500 && dy>-500) {
+
+        int d;
+        if(distance<400) {
+            z=0;
+            while(1) {
+                d=aimTheta[n]-gyro.getAngle();
+                if(d>-10 && d<10) {
+                    break;
+                }
+                time.reset();
+            }
             n++;
             printf("reach%d\r\n",n);
-            ticker.detach();
-            /*for(int j=0; j<4; j++) {
-                motor[j].stop();
-            }
-            wait(1);*/
-            time.reset();
-            ticker.attach(motorOut,0.05);
         }
 
         if(n>=5) {
@@ -250,7 +245,7 @@
                 motor[j].Sc(0);
             }
             while(1) {
-                
+
                 printf("fin");
                 ticker.detach();
             }