Diff: main.cpp

Revision:

1:6b405e5f18ef

Parent:

0:f0f040a29912

--- a/main.cpp	Wed Aug 03 06:45:31 2016 +0000
+++ b/main.cpp	Wed Jul 12 10:48:13 2017 +0000
@@ -1,11 +1,9 @@
 /* 版本說明: 正方形軌跡追蹤(從邊緣出發)邊長3m
 實驗說明:
-    正式實驗:leader為正方形軌跡
-            followers維持等腰三角形隊形
+    增加sgn項，且dt修正為0.11秒
 
-實驗規格:
-
-07/17測試紀錄:
+08/13測試紀錄:
+    可順利運轉，且有記錄結果，但應該不是正確的(非動態模型結果)。
 
 */
 #include "mbed.h"
@@ -19,6 +17,7 @@
 #define pulsesPerRev 1024
 #define kp 0.7// p gain
 #define kd 0.03// d gain
+#define ksgn 0.001// sgn term gain
 // leader initial pose
 #define x0 0  //[m]
 #define y0 0  //[m]
@@ -61,7 +60,7 @@
 float pwm_w1,pwm_w2,pwm_w3;
 // Differential
 float dy,dx,dtheta;
-float dt = 0.05;
+float dt = 0.11;
 // V command
 float t1 = 7.5;//2*3.75
 float T = 22.5;//6*3.75
@@ -80,6 +79,8 @@
 float xL0,yL0;// initial position of leader
 char kb='p';
 
+int sgn(float);
+
 int main()
 {
     pwm_1.period_us(pwm_period_us);
@@ -243,10 +244,10 @@
         X1_b[1] = X1[1]-f1[1];
         X1_b[2] = X1[2]-f1[2];
 
-        u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-Vc[0]);
-        u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-Vc[1]);
-        u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-Vc[2]);
-        //
+        u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-Vc[0])-ksgn*sgn(V1[0]-Vc[0]);
+        u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-Vc[1])-ksgn*sgn(V1[1]-Vc[1]);
+        u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-Vc[2])-ksgn*sgn(V1[2]-Vc[2]);
+        
         omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta);
         omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta);
         omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta);
@@ -278,9 +279,11 @@
         //print
         //printf ("\n");
         //printf("% .2f|% .2f|% .1f|Xnow|%f\n", Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,clk.read());
-        printf("% .2f|% .2f|% .1f|Xnow|% .2f|% .2f|% .1f|XL|%f\n",
+        printf("% .2f|% .2f|% .1f|Xnow|% .2f|% .2f|% .1f|XL|% .2f|% .2f|% .1f|Vnow|%f\n",
                Current_X[0]*100, Current_X[1]*100,Current_X[2]*180/pi-60,
-               XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
+               XL[0]*100, XL[1]*100, XL[2]*180/pi-60,
+               V1[0]*100, V1[1]*100, V1[2]*180/pi,
+               clk.read());
         //printf("% .1f|% .1f|% .1f|Xc|%f\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60,clk.read());
         ///printf ("% .2f|% .2f|% .1f|Vnow\n", V1[0]*100, V1[1]*100, V1[2]*180/pi);
         //printf("%f\n",clk.read());
@@ -307,105 +310,8 @@
     }
 }
 
-/*
-    switch (sw) {
-        case '1':
-            tempchar = sw;
-            xdir=-1;
-            ydir=-1;
-            sw = '2';
-            break;
-        case '2':
-            tempchar = sw;
-            xdir=0;
-            ydir=-1;
-            sw = '3';
-            break;
-        case '3':
-            tempchar = sw;
-            xdir=1;
-            ydir=-1;
-            sw = '6';
-            break;
-        case '4':
-            tempchar = sw;
-            xdir=-1;
-            ydir=0;
-            sw = '1';
-            break;
-            case '5':
-                tempchar = sw;
-
-                xdir=0;
-                ydir=0;
-                XL[2]=0+pi/3;
-                break;
-        case '6':
-            tempchar = sw;
-            xdir=1;
-            ydir=0;
-            sw = '9';
-            break;
-        case '7':
-            tempchar = sw;
-            xdir=-1;
-            ydir=1;
-            sw = '4';
-            break;
-        case '8':
-            tempchar = sw;
-            xdir=0;
-            ydir=1;
-            sw = '7';
-            break;
-        case '9':
-            tempchar = sw;
-            xdir=1;
-            ydir=1;
-            sw = '8';
-            break;
-        default:
-            xdir=0;
-            ydir=0;
-            XL[2]=0+pi/3;
-            break;
-    }*/
-
-// P&V command compensation
-/*if(s0==false) {
-    if(clk.read()<1) {
-        Vc[0]=0.15;
-        Vc[1]=0.05+0.1*clk.read();
-        Vc[2]=0;
-        //
-        XL[0]=0.15*clk.read();// go to target
-        XL[1]=0.05*clk.read()+0.05*clk.read()*clk.read();
-        XL[2]=0+pi/3;
-    } else if(clk.read()>1 && clk.read()<2) {
-        Vc[0]=0.15;
-        Vc[1]=0.15;
-        Vc[2]=0;
-        //
-        XL[0]=0.15*clk.read();// go to target
-        XL[1]=0.1+0.15*(clk.read()-1);
-        XL[2]=0+pi/3;
-    } else if(clk.read()>2 && clk.read()<3) {
-        Vc[0]=0.15-0.1*(clk.read()-2);
-        Vc[1]=0.15;
-        Vc[2]=0;
-        //
-        XL[0]=0.3+(0.3-0.1*(clk.read()-2))*(clk.read()-2)/2;// go to target
-        XL[1]=0.25+0.15*(clk.read()-2);
-        XL[2]=0+pi/3;
-    } else {
-        Vc[0]=0;
-        Vc[1]=0;
-        Vc[2]=0;
-        //
-        XL[0]=0.4;// go to target
-        XL[1]=0.4;
-        XL[2]=0+pi/3;
-        s0=true;
-        clk.reset();
-    }
-}*/
\ No newline at end of file
+int sgn(float e){
+    if (e>0) return 1;
+    else if (e<0) return -1;
+    else return 0; 
+}
\ No newline at end of file