Revision 34:0cf04acfe422, committed 2016-02-03

Comitter:

soulx

Date:

Wed Feb 03 14:39:39 2016 +0000

Parent:

32:1f81f3e83889

Child:

36:1561b6d61095

Commit message:

?? pid ????; ????????????

Changed in this revision

--- a/BEAR_Reciever.lib	Tue Feb 02 01:31:15 2016 +0000
+++ b/BEAR_Reciever.lib	Wed Feb 03 14:39:39 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/BEaR-lab/code/BEAR_Reciever/#2a7476156519
+https://developer.mbed.org/teams/BEaR-lab/code/BEAR_Reciever/#894bca54ae1a

--- a/PID.lib	Tue Feb 02 01:31:15 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/BEaR-lab/code/PID/#807a17d0a172

--- a/main.cpp	Tue Feb 02 01:31:15 2016 +0000
+++ b/main.cpp	Wed Feb 03 14:39:39 2016 +0000
@@ -2,28 +2,34 @@
 //  Include  //
 #include "mbed.h"
 #include "pinconfig.h"
-#include "PID.h"
+//#include "PID.h"
 #include "Motor.h"
 #include "eeprom.h"
 #include "Receiver.h"
 #include "Motion_EEPROM_Address.h"
+
+#include "pidcontrol.h"
+
 #define EEPROM_DELAY 2
+
+//#define DEBUG_UP
+//#define DEBUG_LOW
 //*****************************************************/
 //--PID parameter--
 //-Upper-//
-float U_Kc;
-float U_Ki_gain;
-float U_Kd_gain;
-float U_Ti;
-float U_Td;
+float U_Kc=0.2;
+float U_Ki_gain=0.0;
+float U_Kd_gain=0.0;
+float U_Ti=0.0;
+float U_Td=0.0;
 float U_Ki=U_Kc*U_Ti;
 float U_Kd=U_Kc*U_Td;
 //-lower-//
-float L_Kc;
-float L_Ki_gain;
-float L_Kd_gain;
-float L_Ti;
-float L_Td;
+float L_Kc=0.2;
+float L_Ki_gain=0.0;
+float L_Kd_gain=0.0;
+float L_Ti=0.0;
+float L_Td=0.0;
 float L_Ki=L_Kc*L_Ti;
 float L_Kd=L_Kc*L_Td;
 //*****************************************************/
@@ -35,8 +41,8 @@
 int16_t MY_ID = 0x02;
 //-- Memorry --
 EEPROM memory(PB_4,PA_8,0);
-uint8_t UpMargin[4];
-uint8_t LowMargin[4];
+float UpMargin;
+float LowMargin;
 uint8_t Height[4];
 uint8_t Wheelpos[4];
 uint8_t Mag[24];
@@ -59,11 +65,20 @@
 Motor Upper(PWM_LU,A_LU,B_LU);
 Motor Lower(PWM_LL,A_LL,B_LL);
 //-- PID --
-int Upper_SetPoint;
-int Lower_SetPoint;
-PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
-PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
+int Upper_SetPoint=20;
+int Lower_SetPoint=8;
+//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
+//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
+
+PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
+PID Low_PID(L_Kc, L_Ti, L_Td);
+
 //*****************************************************/
+void Start_Up();
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
+
+Timer counterUP;
+Timer counterLOW;
 
 DigitalOut myled(LED1);
 
@@ -114,52 +129,198 @@
 void SET_UpperPID()
 {
     Upper.period(0.001);
-    Up_PID.setMode(0);
-    Up_PID.setInputLimits(0,127);
-    Up_PID.setOutputLimits(0,1);
+
+    memory.read(ADDRESS_UP_MARGIN,UpMargin);
+    Up_PID.setMargin(UpMargin);
+
+    memory.read(ADDRESS_UPPER_KP,U_Kc);
+    Up_PID.setKp(U_Kc);
+    memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
+    Up_PID.setKi(U_Ki_gain);
+    memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
+    Up_PID.setKd(U_Kd_gain);
+
+    //Up_PID.setMode(0);
+    //Up_PID.setInputLimits(18,62);
+    //Up_PID.setOutputLimits(0,1);
 }
 //******************************************************/
 void SET_LowerPID()
 {
     Lower.period(0.001);
-    Low_PID.setMode(0);
-    Low_PID.setInputLimits(0,127); // set range
-    Low_PID.setOutputLimits(0,1);
+
+    memory.read(ADDRESS_LOW_MARGIN,LowMargin);
+    Low_PID.setMargin(LowMargin);
+
+    memory.read(ADDRESS_LOWER_KP,L_Kc);
+    Low_PID.setKp(L_Kc);
+    memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
+    Low_PID.setKi(L_Ki_gain);
+
+    memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
+    Low_PID.setKd(L_Kd_gain);
+
+    //Low_PID.setMode(0);
+    //Low_PID.setInputLimits(0,127); // set range
+    //Low_PID.setOutputLimits(0,1);
 }
 //******************************************************/
 void Move_Upper()
 {
     Read_Encoder(EncoderA);
-    Upper_Position = Get_EnValue(data);
-
-    Up_PID.setProcessValue(Upper_Position);
-
-    if(Upper_Position - Upper_SetPoint > 0 ) {
-        dir = 1;
-    }
-    if(Upper_Position - Upper_SetPoint < 0 ) {
-        dir = -1;
-    }
-    Upper.speed(Up_PID.compute() * dir);
+    Upper_Position = (float)Get_EnValue(data);
+#ifdef DEBUG_UP
+    printf("read_encode = 0x%2x \n\r",data);
+    printf("Setpoint = %d\n\r",Upper_SetPoint);
+    printf("Upper_Position = %f\n\r",Upper_Position);
+#endif
+    Up_PID.setCurrent(Upper_Position);
+    float speed =Up_PID.compute();
+#ifdef DEBUG_UP
+    printf("E_n= %f\n\r",Up_PID.getErrorNow());
+    printf("Kp = %f\n\r",Up_PID.getKp());
+    printf("speed = %f\n\n\n\r",speed);
+#endif
+    Upper.speed(speed);
 }
 //******************************************************/
 void Move_Lower()
 {
     Read_Encoder(EncoderB);
-    Lower_Position = Get_EnValue(data);
-
-    Low_PID.setProcessValue(Lower_Position);
+    Lower_Position = (float)Get_EnValue(data);
+    Low_PID.setCurrent(Lower_Position);
+#ifdef DEBUG_LOW
+    printf("read_encode = 0x%2x \n\r",data);
+    printf("Setpoint = %d\n\r",Lower_SetPoint);
+    printf("Upper_Position = %f\n\r",Lower_Position);
+#endif
 
-    if(Lower_Position - Lower_SetPoint > 0 ) {
-        dir = 1;
-    }
-    if(Lower_Position - Lower_SetPoint < 0 ) {
-        dir = -1;
-    }
-    Lower.speed(Low_PID.compute() * dir);
+    float speed =Low_PID.compute();
+#ifdef DEBUG_LOW
+    printf("E_n= %f\n\r",Low_PID.getErrorNow());
+    printf("Kp = %f\n\r",Low_PID.getKp());
+    printf("speed = %f\n\n\n\r",speed);
+#endif
+    Lower.speed(speed);
 }
 //******************************************************/
 
+
+void Rc()
+{
+    myled =1;
+    uint8_t data_array[30];
+    uint8_t id=0;
+    uint8_t ins=0;
+    uint8_t status=0xFF;
+
+
+
+    status = com.ReceiveCommand(&id,data_array,&ins);
+    //PC.printf("status = 0x%02x\n\r",status);
+    if(status == ANDANTE_ERRBIT_NONE) {
+        CmdCheck((int16_t)id,data_array,ins);
+    }
+
+}
+/*******************************************************/
+int main()
+{
+    PC.baud(115200);
+    /*
+    while(1)
+    {
+    Read_Encoder(EncoderA);
+    Upper_Position = Get_EnValue(data);
+    Read_Encoder(EncoderB);
+    Lower_Position = Get_EnValue(data);
+    PC.printf("Upper Position : %f\n",Upper_Position);
+    PC.printf("Lower_Position : %f\n",Lower_Position);
+    wait(0.5);
+    }
+    */
+
+
+    //Recieve.attach(&Rc,0.025);
+    //Start_Up();
+
+    SET_UpperPID();
+    SET_LowerPID();
+
+    printf("BEAR MOTION ID:0x%02x\n\r",MY_ID);
+    /*
+    while(1)
+    {
+
+        Upper.speed(-1);
+        wait_ms(400);
+        Upper.speed(1);
+        wait_ms(400);
+        Upper.break();
+
+        Lower.speed(-1.0);
+        wait_ms(401);
+        Lower.speed(1.0);
+        wait_ms(401);
+        Lower.break();
+        }
+    */
+
+    //  counterUP.start();
+//    counterLOW.start();
+
+    while(1) {
+
+        /*
+        //printf("timer = %d\n\r",counter.read_ms());
+        if(counterUP.read_ms() > 1400) {
+
+            if(Upper_SetPoint < 53) {
+                Upper_SetPoint++;
+            } else
+                Upper_SetPoint =18;
+
+            counterUP.reset();
+
+        }
+
+        if(counterLOW.read_ms() > 700) {
+
+            if(Lower_SetPoint < 100) {
+                Lower_SetPoint++;
+            } else
+                Lower_SetPoint =8;
+
+            counterLOW.reset();
+
+        }
+        */
+        myled =0;
+        //wait_ms(10);
+///////////////////////////////////////////////////// start
+        //Set Set_point
+        Up_PID.setGoal(Upper_SetPoint);
+        Low_PID.setGoal(Lower_SetPoint);
+
+        //Control Motor
+        Move_Upper();
+        Move_Lower();
+/////////////////////////////////////////////////////  stop =306us
+        //uint8_t aaa[1]={10};
+        //com.sendBodyWidth(0x01,aaa);
+        Rc();
+        //wait_ms(1);
+    }
+}
+
+
+
+
+
+
+
+
+
 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
 {
     if(id==MY_ID) {
@@ -180,27 +341,42 @@
 
                         IntUpAngle[0]=command[1];
                         IntUpAngle[1]=command[2];
-                        Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
+                        Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
                         //printf("Up Angle = %f\n",up_angle);
                         IntLowAngle[0]=command[5];
                         IntLowAngle[1]=command[6];
-                        Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
+                        Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
                         //printf("Low Angle = %f\n",low_angle);
                         break;
                     }
                     case UP_MARGIN: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            UpMargin[i]=command[1+i];
-                            //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]);
-                        }
+                        uint8_t int_buffer[2];
+                        uint8_t float_buffer[2];
+                        float Int,Float;
+                        int_buffer[0]=command[1];
+                        int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
+                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
+                        UpMargin=Int+Float;
+                        Up_PID.setMargin(UpMargin);
+                        //printf("Kp Upper : %f\r\n",U_Kc);
                         break;
                     }
                     case LOW_MARGIN: {
-                        int i;
-                        for(i=0; i<4; i++) {
-                            LowMargin[i]=command[1+i];
-                        }
+                        uint8_t int_buffer[2];
+                        uint8_t float_buffer[2];
+                        float Int,Float;
+                        int_buffer[0]=command[1];
+                        int_buffer[1]=command[2];
+                        float_buffer[0]=command[3];
+                        float_buffer[1]=command[4];
+                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+                        Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
+                        LowMargin=Int+Float;
+                        Low_PID.setMargin(LowMargin);
+                        //printf("Kp Upper : %f\r\n",U_Kc);
                         break;
                     }
                     case KP_UPPER_MOTOR: {
@@ -215,6 +391,7 @@
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         U_Kc=Int+Float;
                         //printf("Kp Upper : %f\r\n",U_Kc);
+                        Up_PID.setKp(U_Kc);
                         break;
                     }
                     case KI_UPPER_MOTOR: {
@@ -228,7 +405,8 @@
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         U_Ki_gain=Int+Float;
-                        U_Ti=U_Ki_gain/U_Kc;
+                        //U_Ti=U_Ki_gain;
+                        Up_PID.setKi(U_Ki_gain);
                         //printf("Ki Upper : %f\r\n",U_Ki_gain);
                         break;
                     }
@@ -243,7 +421,8 @@
                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
                         U_Kd_gain=Int+Float;
-                        U_Td=U_Kd_gain/U_Kc;
+                        Up_PID.setKd(U_Kd_gain);
+                        //U_Td=U_Kd_gain/U_Kc;
                         //printf("Kd Upper : %f\r\n",U_Kd_gain);
                         break;
                     }
@@ -431,16 +610,12 @@
                             wait_ms(EEPROM_DELAY);
 
                         } else if(command[1]==UP_MARGIN) {
-                            int32_t data_buff;
-                            data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin);
-                            memory.write(ADDRESS_UP_MARGIN,data_buff);
+                            memory.write(ADDRESS_UP_MARGIN,UpMargin);
                             wait_ms(EEPROM_DELAY);
                             //printf("save OK!!\n\r");
 
                         } else if (command[1]==LOW_MARGIN) {
-                            int32_t data_buff;
-                            data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin);
-                            memory.write(ADDRESS_LOW_MARGIN,data_buff);
+                            memory.write(ADDRESS_LOW_MARGIN,LowMargin);
                             wait_ms(EEPROM_DELAY);
 
                         } else if (command[1]==PID_UPPER_MOTOR) {
@@ -526,16 +701,12 @@
                         break;
                     }
                     case UP_MARGIN: {
-                        int32_t data_buff;
-                        memory.read(ADDRESS_UP_MARGIN,data_buff);
-                        Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin);
+                        memory.read(ADDRESS_UP_MARGIN,UpMargin);
                         com.sendUpMargin(MY_ID,UpMargin);
                         break;
                     }
                     case LOW_MARGIN: {
-                        int32_t data_buff;
-                        memory.read(ADDRESS_LOW_MARGIN,data_buff);
-                        Utilities::ConvertInt32ToUInt8Array(data_buff,LowMargin);
+                        memory.read(ADDRESS_LOW_MARGIN,LowMargin);
                         com.sendLowMargin(MY_ID,LowMargin);
                         break;
                     }
@@ -695,70 +866,4 @@
     memory.read(ADDRESS_LOWER_KD,L_Td);
 
 }
-/*******************************************************/
-void Rc()
-{
-    myled =1;
-    uint8_t data_array[30];
-    uint8_t id=0;
-    uint8_t ins=0;
-    uint8_t status=0xFF;
-
-
-
-    status = com.ReceiveCommand(&id,data_array,&ins);
-    //PC.printf("status = 0x%02x\n\r",status);
-    if(status == ANDANTE_ERRBIT_NONE) {
-        CmdCheck((int16_t)id,data_array,ins);
-    }
-
-}
-/*******************************************************/
-int main()
-{
-    PC.baud(115200);
-    /*
-    while(1)
-    {
-    Read_Encoder(EncoderA);
-    Upper_Position = Get_EnValue(data);
-    Read_Encoder(EncoderB);
-    Lower_Position = Get_EnValue(data);
-    PC.printf("Upper Position : %f\n",Upper_Position);
-    PC.printf("Lower_Position : %f\n",Lower_Position);
-    wait(0.5);
-    }
-    */
-
-
-    //Recieve.attach(&Rc,0.025);
-    //Start_Up();
-
-    SET_UpperPID();
-    SET_LowerPID();
-
-    printf("BEAR MOTION\n\r");
-    while(1) {
-        myled =0;
-        //wait_ms(10);
-///////////////////////////////////////////////////// start
-        //Set Set_point
-        Up_PID.setSetPoint(Upper_SetPoint);
-        Low_PID.setSetPoint(Lower_SetPoint);
-
-        //Control Motor
-        Move_Upper();
-        Move_Lower();
-/////////////////////////////////////////////////////  stop =306us
-        //uint8_t aaa[1]={10};
-        //com.sendBodyWidth(0x01,aaa);
-        Rc();
-        //wait_ms(1);
-    }
-}
-
-
-
-
-
-
+/*****************************************************/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.cpp	Wed Feb 03 14:39:39 2016 +0000
@@ -0,0 +1,168 @@
+#include "pidcontrol.h"
+
+PID::PID()
+{
+    Kp=1.0f;
+    Ki=0.0f;
+    Kd=0.0f;
+    il=65535.0;
+    margin = 0.0f;
+
+}
+
+PID::PID(float p,float i,float d)
+{
+    Kp=p;
+    Ki=i;
+    Kd=d;
+    il=65535.0;
+    margin =0.0f;
+}
+
+void PID::setGoal(float ref)
+{
+    setpoint = ref;
+}
+
+void PID::setCurrent(float sensor)
+{
+    input = sensor;
+}
+
+float PID::compute()
+{
+
+    e_n = setpoint - input;
+
+    if((e_i < il) && (e_i > -il))
+    {
+        e_i += e_n;
+    }
+    else
+    {
+#ifdef PID_DEBUG
+        printf("il overflow\n\r");
+#endif
+        e_i =il;
+    }
+
+
+    output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1));
+
+    if(output > 0)
+    {
+        if(output < margin)
+        {
+            output = 0.0;
+        }
+    }
+    else
+    {
+        if(output > -margin)
+        {
+            output = 0.0;
+        }
+    }
+
+    return output;
+}
+
+void PID::setMargin(float gap)
+{
+    margin =gap;
+}
+
+float PID::getMargin()
+{
+    return margin;
+}
+
+
+void PID::setIntegalLimit(float limit)
+{
+    il = limit;
+}
+float PID::getIntegalLimit()
+{
+    return il;
+}
+
+
+float PID::getErrorNow()
+{
+    return e_n;
+}
+
+float PID::getErrorLast()
+{
+    return e_n_1;
+}
+
+float PID::getErrorDiff()
+{
+    return e_n - e_n_1;
+}
+
+float PID::getErrorIntegal()
+{
+    return e_i;
+}
+
+void PID::setKp(float p)
+{
+    if(p < 0.0f)
+    {
+#ifdef PID_DEBUG
+        printf("Kp = 0.0\n\r");
+#endif
+        Kp=0.0;
+    }
+    else
+    {
+        Kp=p;
+    }
+}
+
+void PID::setKi(float i)
+{
+    if(i < 0.0f)
+    {
+#ifdef PID_DEBUG
+        printf("Ki = 0.0\n\r");
+#endif
+        Ki=0.0;
+    }
+    else
+    {
+        Ki=i;
+    }
+}
+void PID::setKd(float d)
+{
+    if(d < 0.0f)
+    {
+#ifdef PID_DEBUG
+        printf("Kd = 0.0\n\r");
+#endif
+        Kd=0.0;
+    }
+    else
+    {
+        Kd=d;
+    }
+}
+
+float PID::getKp()
+{
+    return Kp;    
+}
+
+float PID::getKi()
+{   
+    return Ki;    
+}
+
+float PID::getKd()
+{   
+    return Kd;    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.h	Wed Feb 03 14:39:39 2016 +0000
@@ -0,0 +1,49 @@
+#ifndef _PIDCONTROL_H_
+#define _PIDCONTROL_H_
+
+#include "mbed.h"
+
+class PID{
+    public:
+        PID();
+        PID(float p,float i,float d);
+        void setGoal(float ref);
+        //float getGoal();    
+        void setCurrent(float sensor);
+        float compute();
+        
+        void setMargin(float gap);
+        float getMargin();
+        void setIntegalLimit(float limit);
+        float getIntegalLimit();
+        
+        float getErrorNow();
+        float getErrorLast();
+        float getErrorDiff();
+        float getErrorIntegal();
+        
+        void setKp(float);
+        void setKi(float);
+        void setKd(float);
+        
+        float getKp();
+        float getKi();
+        float getKd();
+        
+    private:
+        float e_n;      //error now
+        float e_n_1;    //error last time
+        float e_i;      //error integal
+        float il;       //integal limit
+        float margin;    //output margin
+        
+        float Kp,Ki,Kd;    
+ 
+        float setpoint;
+        float input;    
+        float output;
+};
+    
+
+
+#endif
\ No newline at end of file