sampleProgram

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_fuzi by kusano kiyoshige

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers encorder_.h Source File

encorder_.h

00001 #define PI      3.141592654f
00002 #define Nmax    18000
00003 #define invalidN    0.1
00004 
00005 class Position_pid_enc {
00006 public  :
00007     void set_parameter(float Kp, float Ki, float Kd) {
00008         kp = Kp;
00009         ki = Ki;
00010         kd = Kd;
00011     }
00012 
00013     void position_cal(float target, float nowval, float dt) {
00014         old = now;
00015         now = nowval - target;
00016         p = now;
00017         i = i + (now + old)/2.0f * dt;
00018         d = (now - old) / dt;
00019         result = kp*p + ki*i + kd*d;
00020         if (result > 1.0f) {
00021             result = 1.0f;
00022         } else if (result < -1.0f) {
00023             result = -1.0f;
00024         }
00025     }
00026 
00027     float duty() {
00028         return result;
00029     }
00030     
00031     float duty_enableWidth(float duty_min, float duty_max){
00032         if(now > duty_min && now < duty_max){   //nowは差
00033             return 0.0;
00034         }else{
00035             return result;
00036         }
00037     }
00038 
00039 private :
00040     float kp, ki, kd,
00041           old, now,
00042           p, i, d, result,
00043           duty_min, duty_max;
00044 };
00045 /*
00046 class Speed_pid
00047 {
00048 public:
00049     void set_parameter(float Kp, float Ki, float Kd) {
00050         kp = Kp;
00051         ki = Ki;
00052         kd = Kd;
00053     }
00054     
00055     float duty(){
00056         return result;
00057     }
00058     
00059     float e,p,i,d;
00060 
00061 protected :
00062     float kp,ki,kd;
00063     float e1,e2;
00064     //float p,i,d;
00065     float result;
00066     
00067     void pid_cal(float target, float val, float dt){
00068         e2 = e1;
00069         e1 = e;
00070         e = target - val;
00071         p = e - e1;
00072         i = e;
00073         d = (e-2*e1+e2);
00074         result = result + (kp*p+ki*i+kd*d);
00075         if (result > 1.0f) {
00076             result = 1.0f;
00077         } else if (result < -1.0f) {
00078             result = -1.0f;
00079         }
00080         if ((target == 0) && (fabs(e) < invalidN)) {
00081             result = 0;
00082         }
00083     }  
00084 };
00085 */
00086 class Encoder : public Position_pid_enc //: public Speed_pid
00087 {
00088 public:
00089     Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) {
00090     }
00091 
00092     void setup(int Ppr) {
00093         ppr = Ppr;
00094         A.rise(this, &Encoder::A_count);
00095         B.rise(this, &Encoder::B_count);
00096         origin();
00097     }
00098     
00099     void cal(float target, float dt){
00100         radian = (float) count/ppr*2*PI;
00101         degree = radian / PI * 180.0;
00102         w = (radian - oldtheta) / dt;
00103         v = 0.05*w/27;
00104         n = w*30/(PI);
00105         oldtheta = radian;
00106         //pid_cal(target, n/Nmax, dt);
00107         //deg_cylinder = (count / 1200) * 360 * 3 / 20;
00108         position_cal(target, (float)degree, dt);
00109     }
00110 
00111     float deg(){
00112         return degree;
00113     }
00114     
00115     float get_deg_cylinder(){
00116         return deg_cylinder;
00117     }
00118     
00119     float rad() {
00120         return radian;
00121     }
00122     
00123     int pulse(){
00124         return count;
00125     }
00126     
00127     float N(){
00128         return n;
00129     }
00130 
00131     void origin() {
00132         count = 0;
00133     }
00134 
00135 private :
00136     InterruptIn A;
00137     InterruptIn B;
00138 
00139     int ppr;
00140     signed int count;
00141     float radian,oldtheta;
00142     float w,v,n;
00143     float degree, deg_cylinder;
00144 
00145     void A_count() {
00146         count ++;
00147     }
00148 
00149     void B_count() {
00150         count --;
00151     }
00152 };