kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

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

encorder.h@10:04f2a82cfd89, 2017-07-16 (annotated)

Committer:
echo_piyo
Date:
Sun Jul 16 04:06:49 2017 +0000
Revision:
10:04f2a82cfd89
Child:
31:285c9898da03
(??)????????????encorder.h?Speed_pid??Position_pid?????Main?????setup????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 10:04f2a82cfd89 1 #define PI 3.141592654f
echo_piyo 10:04f2a82cfd89 2 #define Nmax 18000
echo_piyo 10:04f2a82cfd89 3 #define invalidN 0.1
echo_piyo 10:04f2a82cfd89 4
echo_piyo 10:04f2a82cfd89 5 class Position_pid_enc {
echo_piyo 10:04f2a82cfd89 6 public :
echo_piyo 10:04f2a82cfd89 7 void set_parameter(float Kp, float Ki, float Kd) {
echo_piyo 10:04f2a82cfd89 8 kp = Kp;
echo_piyo 10:04f2a82cfd89 9 ki = Ki;
echo_piyo 10:04f2a82cfd89 10 kd = Kd;
echo_piyo 10:04f2a82cfd89 11 }
echo_piyo 10:04f2a82cfd89 12
echo_piyo 10:04f2a82cfd89 13 void position_cal(float target, float nowval, float dt) {
echo_piyo 10:04f2a82cfd89 14 old = now;
echo_piyo 10:04f2a82cfd89 15 now = nowval - target;
echo_piyo 10:04f2a82cfd89 16 p = now;
echo_piyo 10:04f2a82cfd89 17 i = i + (now + old)/2.0f * dt;
echo_piyo 10:04f2a82cfd89 18 d = (now - old) / dt;
echo_piyo 10:04f2a82cfd89 19 result = kp*p + ki*i + kd*d;
echo_piyo 10:04f2a82cfd89 20 if (result > 1.0f) {
echo_piyo 10:04f2a82cfd89 21 result = 1.0f;
echo_piyo 10:04f2a82cfd89 22 } else if (result < -1.0f) {
echo_piyo 10:04f2a82cfd89 23 result = -1.0f;
echo_piyo 10:04f2a82cfd89 24 }
echo_piyo 10:04f2a82cfd89 25 }
echo_piyo 10:04f2a82cfd89 26
echo_piyo 10:04f2a82cfd89 27 float duty() {
echo_piyo 10:04f2a82cfd89 28 return result;
echo_piyo 10:04f2a82cfd89 29 }
echo_piyo 10:04f2a82cfd89 30
echo_piyo 10:04f2a82cfd89 31 private :
echo_piyo 10:04f2a82cfd89 32 float kp, ki, kd,
echo_piyo 10:04f2a82cfd89 33 old, now,
echo_piyo 10:04f2a82cfd89 34 p, i, d, result;
echo_piyo 10:04f2a82cfd89 35 };
echo_piyo 10:04f2a82cfd89 36 /*
echo_piyo 10:04f2a82cfd89 37 class Speed_pid
echo_piyo 10:04f2a82cfd89 38 {
echo_piyo 10:04f2a82cfd89 39 public:
echo_piyo 10:04f2a82cfd89 40 void set_parameter(float Kp, float Ki, float Kd) {
echo_piyo 10:04f2a82cfd89 41 kp = Kp;
echo_piyo 10:04f2a82cfd89 42 ki = Ki;
echo_piyo 10:04f2a82cfd89 43 kd = Kd;
echo_piyo 10:04f2a82cfd89 44 }
echo_piyo 10:04f2a82cfd89 45
echo_piyo 10:04f2a82cfd89 46 float duty(){
echo_piyo 10:04f2a82cfd89 47 return result;
echo_piyo 10:04f2a82cfd89 48 }
echo_piyo 10:04f2a82cfd89 49
echo_piyo 10:04f2a82cfd89 50 float e,p,i,d;
echo_piyo 10:04f2a82cfd89 51
echo_piyo 10:04f2a82cfd89 52 protected :
echo_piyo 10:04f2a82cfd89 53 float kp,ki,kd;
echo_piyo 10:04f2a82cfd89 54 float e1,e2;
echo_piyo 10:04f2a82cfd89 55 //float p,i,d;
echo_piyo 10:04f2a82cfd89 56 float result;
echo_piyo 10:04f2a82cfd89 57
echo_piyo 10:04f2a82cfd89 58 void pid_cal(float target, float val, float dt){
echo_piyo 10:04f2a82cfd89 59 e2 = e1;
echo_piyo 10:04f2a82cfd89 60 e1 = e;
echo_piyo 10:04f2a82cfd89 61 e = target - val;
echo_piyo 10:04f2a82cfd89 62 p = e - e1;
echo_piyo 10:04f2a82cfd89 63 i = e;
echo_piyo 10:04f2a82cfd89 64 d = (e-2*e1+e2);
echo_piyo 10:04f2a82cfd89 65 result = result + (kp*p+ki*i+kd*d);
echo_piyo 10:04f2a82cfd89 66 if (result > 1.0f) {
echo_piyo 10:04f2a82cfd89 67 result = 1.0f;
echo_piyo 10:04f2a82cfd89 68 } else if (result < -1.0f) {
echo_piyo 10:04f2a82cfd89 69 result = -1.0f;
echo_piyo 10:04f2a82cfd89 70 }
echo_piyo 10:04f2a82cfd89 71 if ((target == 0) && (fabs(e) < invalidN)) {
echo_piyo 10:04f2a82cfd89 72 result = 0;
echo_piyo 10:04f2a82cfd89 73 }
echo_piyo 10:04f2a82cfd89 74 }
echo_piyo 10:04f2a82cfd89 75 };
echo_piyo 10:04f2a82cfd89 76 */
echo_piyo 10:04f2a82cfd89 77 class Encoder : public Position_pid_enc //: public Speed_pid
echo_piyo 10:04f2a82cfd89 78 {
echo_piyo 10:04f2a82cfd89 79 public:
echo_piyo 10:04f2a82cfd89 80 Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) {
echo_piyo 10:04f2a82cfd89 81 }
echo_piyo 10:04f2a82cfd89 82
echo_piyo 10:04f2a82cfd89 83 void setup(int Ppr) {
echo_piyo 10:04f2a82cfd89 84 ppr = Ppr;
echo_piyo 10:04f2a82cfd89 85 A.rise(this, &Encoder::A_count);
echo_piyo 10:04f2a82cfd89 86 B.rise(this, &Encoder::B_count);
echo_piyo 10:04f2a82cfd89 87 origin();
echo_piyo 10:04f2a82cfd89 88 }
echo_piyo 10:04f2a82cfd89 89
echo_piyo 10:04f2a82cfd89 90 void cal(float target, float dt) {
echo_piyo 10:04f2a82cfd89 91 radian = (float) count/ppr*2*PI;
echo_piyo 10:04f2a82cfd89 92 degree = radian / PI * 180.0; //
echo_piyo 10:04f2a82cfd89 93 w = (radian - oldtheta) / dt;
echo_piyo 10:04f2a82cfd89 94 v = 0.05*w/27;
echo_piyo 10:04f2a82cfd89 95 n = w*30/(PI);
echo_piyo 10:04f2a82cfd89 96 oldtheta = radian;
echo_piyo 10:04f2a82cfd89 97 //pid_cal(target, n/Nmax, dt);
echo_piyo 10:04f2a82cfd89 98 deg_cylinder_cal(); //
echo_piyo 10:04f2a82cfd89 99 position_cal(target, deg_cylinder, dt);
echo_piyo 10:04f2a82cfd89 100 }
echo_piyo 10:04f2a82cfd89 101
echo_piyo 10:04f2a82cfd89 102 void deg_cylinder_cal(){
echo_piyo 10:04f2a82cfd89 103 deg_cylinder = (count / 1200) * 360 * 3 / 20;
echo_piyo 10:04f2a82cfd89 104 }
echo_piyo 10:04f2a82cfd89 105
echo_piyo 10:04f2a82cfd89 106 float deg(){
echo_piyo 10:04f2a82cfd89 107 return degree;
echo_piyo 10:04f2a82cfd89 108 }
echo_piyo 10:04f2a82cfd89 109
echo_piyo 10:04f2a82cfd89 110 float rad() {
echo_piyo 10:04f2a82cfd89 111 return radian;
echo_piyo 10:04f2a82cfd89 112 }
echo_piyo 10:04f2a82cfd89 113
echo_piyo 10:04f2a82cfd89 114 long pulse(){
echo_piyo 10:04f2a82cfd89 115 return count;
echo_piyo 10:04f2a82cfd89 116 }
echo_piyo 10:04f2a82cfd89 117
echo_piyo 10:04f2a82cfd89 118 float N(){
echo_piyo 10:04f2a82cfd89 119 return n;
echo_piyo 10:04f2a82cfd89 120 }
echo_piyo 10:04f2a82cfd89 121
echo_piyo 10:04f2a82cfd89 122 void origin() {
echo_piyo 10:04f2a82cfd89 123 count = 0;
echo_piyo 10:04f2a82cfd89 124 }
echo_piyo 10:04f2a82cfd89 125
echo_piyo 10:04f2a82cfd89 126 private :
echo_piyo 10:04f2a82cfd89 127 InterruptIn A;
echo_piyo 10:04f2a82cfd89 128 InterruptIn B;
echo_piyo 10:04f2a82cfd89 129
echo_piyo 10:04f2a82cfd89 130 int ppr;
echo_piyo 10:04f2a82cfd89 131 signed long count;
echo_piyo 10:04f2a82cfd89 132 float radian,oldtheta;
echo_piyo 10:04f2a82cfd89 133 float w,v,n;
echo_piyo 10:04f2a82cfd89 134 float degree, deg_cylinder;
echo_piyo 10:04f2a82cfd89 135
echo_piyo 10:04f2a82cfd89 136 void A_count() {
echo_piyo 10:04f2a82cfd89 137 count ++;
echo_piyo 10:04f2a82cfd89 138 }
echo_piyo 10:04f2a82cfd89 139
echo_piyo 10:04f2a82cfd89 140 void B_count() {
echo_piyo 10:04f2a82cfd89 141 count --;
echo_piyo 10:04f2a82cfd89 142 }
echo_piyo 10:04f2a82cfd89 143 };