ライブラリ化を行った後
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_Practice1 by
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 };
Generated on Fri Jul 15 2022 04:29:42 by
1.7.2
