Revision 1:ad8acc6cfba7, committed 2015-10-30

Comitter:

kikoaac

Date:

Fri Oct 30 12:48:08 2015 +0000

Parent:

0:8719ed91e6fd

Commit message:

???
;

Changed in this revision

--- a/main.cpp	Wed Sep 23 06:06:49 2015 +0000
+++ b/main.cpp	Fri Oct 30 12:48:08 2015 +0000
@@ -11,7 +11,7 @@
 #include "fusion_Motor.h"
 #include "Pin.h"
 #define Lx 0.0001
-#define Ku 0.32200//0.922000
+#define Ku 0.48000//0.922000
 #define Pu 0.0125
 #define Kp Ku*0.6
 #define Ti Pu*0.5
@@ -27,6 +27,7 @@
 #define Databit 0
 #define TIME 0.0125
 Timer Time;
+DigitalOut Elec(D15);
 /*
 PID p(P,I,D,&Time);
 QEI i(QEI1_A,QEI1_B,NC,50,&Time);
@@ -34,7 +35,7 @@
 fusion_Motor mecanam3(i,p,d);
 */
 
-
+float duty_max;
 Serial device(D8,D2);
 /*
 PID *pid;
@@ -43,7 +44,7 @@
 */
 PID pid[4] = {PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time)};
 QEI wheel[4] = {QEI(QEI1_A,QEI1_B,NC,100,&Time),QEI(QEI2_A,QEI2_B,NC,100,&Time),QEI(QEI3_A,QEI3_B,NC,100,&Time),QEI(QEI4_A,QEI4_B,NC,100,&Time)};
-Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D5,D4,D3,PB_8,PB_9),Motor(D12,D11,D10,D9,D7)};
+Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D7,D6,D5,D4,D3),Motor(D13,D12,D11,D10,D9)};
 fusion_Motor mecanam[4] = {
     fusion_Motor(wheel[0],pid[0],motor[0]),//front right
     fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
@@ -98,7 +99,6 @@
 enum STATE {
     R,L,F,B
 } state;
-PwmOut LED(LED1);
 #define PI 3.141592653589793
 float DUTY_fill[4]= {0};
 float DUTY_inte[4]= {0};
@@ -113,7 +113,7 @@
     *data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte;
     *fill = (dat - *prev_dat)*inter;
     *prev_dat  =dat;
-    
+
     /*
     if(tardata>*data)
         *data+=0.01f;
@@ -135,7 +135,16 @@
 double mecCot(float angle,STATE RorL)
 {
     double radian = (double)angle * PI / 180.0;
-    return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4-PI/2) : 1.4142135623730950488016887242097*cos(radian-PI/4-PI/2);
+    return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4) : 1.4142135623730950488016887242097*cos(radian-PI/4);
+}
+bool stop_flag()
+{
+    bool flag=1;
+    for(int i=0;i<4;i++)
+    {
+        flag &= (DUTY[i] <duty_max*0.8f && DUTY[i]>-duty_max*0.8f);
+    }
+    return flag;
 }
 void vector(float angle)
 {
@@ -163,7 +172,12 @@
 void pid_reset()
 {
     for(int i=0; i<4; i++)
-        pid[i].pid_reset();
+    {
+     DUTY_fill[i]= 0;
+     DUTY_inte[i]= 0;
+     DUTY[i]= 0;
+     DUTY_prev[i]= 0;
+     }
 }
 void getData()
 {
@@ -176,20 +190,33 @@
     //printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
     //printf("getdata!!\r\n");
 }
+int flag=0;
 bool MotorState()
 {
     for(int a=0; a<4; a++) {
         if(speed==0) {
-            mecanam[a].run(Free,1);
+            mecanam[a].run(Stop,1);
+            pid_reset();
+            //mecanam[a].pid_reset();
         } else if(speed==1) {
-            mecanam[a].run(Stop,1);
-
+            if(stop_flag())
+            {
+                mecanam[a].run(Stop,1.0);
+                pid_reset();
+            }
+            else 
+            {                            
+                motruns(R,0);
+                motruns(L,0);
+            }   
         } else if(speed==2) {
-            mecanam[a].duty_max(0.3);
-            mecanam[a].duty_min(-0.3);
+            duty_max=0.2f;
+            mecanam[a].duty_max(duty_max);
+            mecanam[a].duty_min(-duty_max);
         } else if(speed==3) {
-            mecanam[a].duty_max(1.0);
-            mecanam[a].duty_min(-1.0);
+            duty_max=1.0f;
+            mecanam[a].duty_max(duty_max);
+            mecanam[a].duty_min(-duty_max);
         }
     }
     return speed==0||speed==1;
@@ -207,14 +234,17 @@
 }
 int main()
 {
+    Elec=0;
     for(int i=0; i<4; i++) {
         mecanam[i](fusion_Motor::Normal,1);
         mecanam[i].run(Free,1.0);
+        mecanam[i].setbias(0.0);
+        mecanam[i].setbias_motor(0.0);
     }
-for(int a=0;a<4;a++){
-    mecanam[a].duty_max(1.0);
-    mecanam[a].duty_min(-1.0);}
-    led=0.5;
+    for(int a=0; a<4; a++) {
+        mecanam[a].duty_max(1.0);
+        mecanam[a].duty_min(-1.0);
+    }
     //M=0x1f;
     //wait(3);
     //setup();
@@ -225,28 +255,61 @@
     bool prev_sot=0;
 
     wait(1);
-    Ticker tic;
-    tic.attach(&f,3);
+//    Ticker tic;
+//    tic.attach(&f,3);
     //mecanam3=200;
     //for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
     //for(int i = 0 ;i<4;i++)mecanam[i]=100;
     //wait(5);
     DigitalIn PIN(USER_BUTTON);
+    if(PIN==0) {
+        
+        bool flag=0;
+        float duty=1;
+        while(1) {
+            for(int i=0;i<4;i++)
+            mecanam[i].run(Front,1);
+            wait(4);
+            for(int i=0;i<4;i++)
+            mecanam[i].run(Front,0);
+            wait(4);
+            /*if(PIN==0&&flag==1) {
+                flag=0;
+                duty-=0.1;
+            } else if(PIN==1&&flag==0) {
+                flag=1;
+                for(int i=0; i<4; i++)
+                    mecanam[i].run(Front,duty);
+            }*/
+        }
+    }
+    flag=PIN;
+    int prevang=0;
+    Timer waittimer;
     while(1) {
-       //printf("start main\r\n");
-     
+        //Elec=!PIN;
+        /*
+        for(int x = 0; 1; x++) {
+        vector(x);
+            for(int i=0; i<4; i++)
+                printf("%f   ",mecanam[i].getduty());
+            printf("\r\n");
+            }
+            */
+        //printf("start main\r\n");
         for(int x = 0; 1; x++) {
             if(PIN==0)GAIN_P+=0.001;
-        //printf("start main\r\n");
-        /*vector(Y);
-            for(int i=0; i<4; i++)
-                printf("%f   ",DUTY[i]);
-            printf("%F \r\n",GAIN_P);*/
+            //printf("start main\r\n");
+            /*
+            vector(Y);
+                for(int i=0; i<4; i++)
+                    printf("%f   ",DUTY[i]);
+                printf("%F \r\n",GAIN_P);*/
             wait(TIME);
             //mecanam3=mecCot(i,R);
             //wait(0.5);
             //printf("%5d, %5.5f , %5.5f, %5.5f\r\n",i,mecanam3.getrpm(),mecanam3.rerpmper(mecCot(i,R)),mecanam3.rpmper(300*mecCot(i,R)));
-            
+
             getData();
             //wait(0.01);
             if(MotorState())break;
@@ -255,6 +318,7 @@
             if(sot==0) {
                 prev_sot=sot;
                 float angle = Data2*11.25;
+                prevang=Data2;
                 vector(angle);
             } else if(sot==1) {
                 if(Data2==0) {
@@ -262,12 +326,15 @@
                         mecanam[i]=1;
                         mecanam[i+2]=-1;
                     }
-                }
-                else if(Data2==1) {
+                } else if(Data2==1) {
                     for(int i=0; i<2; i++) {
                         mecanam[i]=-1;
                         mecanam[i+2]=1;
                     }
+                } else if(Data2==2){
+                    Elec=1;
+                } else if(Data2==3){
+                    Elec=0;
                 } else {
                     for(int i=0; i<4; i++)
                         mecanam[i].run(Stop,1);
@@ -278,3 +345,4 @@
 }
 
 
+

--- a/motor_lib/Motor.lib	Wed Sep 23 06:06:49 2015 +0000
+++ b/motor_lib/Motor.lib	Fri Oct 30 12:48:08 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/kikoaac/code/Motor/#dea2df71cb97
+http://developer.mbed.org/users/kikoaac/code/Motor/#56a1159c881c

--- a/motor_lib/PID.lib	Wed Sep 23 06:06:49 2015 +0000
+++ b/motor_lib/PID.lib	Fri Oct 30 12:48:08 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/kikoaac/code/PID/#3519920d064d
+http://developer.mbed.org/users/kikoaac/code/PID/#775c9421fe3b