![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
headerfile_unuse/encorder_.h@76:03475cafb326, 2017-10-22 (annotated)
- Committer:
- echo_piyo
- Date:
- Sun Oct 22 11:26:25 2017 +0000
- Revision:
- 76:03475cafb326
- Parent:
- 66:1664ee92539d
(??)????shoulderCalclation???controllerEvent???output?????; ????????????
Who changed what in which revision?
User | Revision | Line number | New 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 | 49:b041c815c063 | 30 | |
echo_piyo | 49:b041c815c063 | 31 | float duty_enableWidth(float duty_min, float duty_max){ |
echo_piyo | 49:b041c815c063 | 32 | if(now > duty_min && now < duty_max){ //nowは差 |
echo_piyo | 49:b041c815c063 | 33 | return 0.0; |
echo_piyo | 49:b041c815c063 | 34 | }else{ |
echo_piyo | 49:b041c815c063 | 35 | return result; |
echo_piyo | 49:b041c815c063 | 36 | } |
echo_piyo | 49:b041c815c063 | 37 | } |
echo_piyo | 10:04f2a82cfd89 | 38 | |
echo_piyo | 10:04f2a82cfd89 | 39 | private : |
echo_piyo | 10:04f2a82cfd89 | 40 | float kp, ki, kd, |
echo_piyo | 10:04f2a82cfd89 | 41 | old, now, |
echo_piyo | 49:b041c815c063 | 42 | p, i, d, result, |
echo_piyo | 49:b041c815c063 | 43 | duty_min, duty_max; |
echo_piyo | 10:04f2a82cfd89 | 44 | }; |
echo_piyo | 10:04f2a82cfd89 | 45 | /* |
echo_piyo | 10:04f2a82cfd89 | 46 | class Speed_pid |
echo_piyo | 10:04f2a82cfd89 | 47 | { |
echo_piyo | 10:04f2a82cfd89 | 48 | public: |
echo_piyo | 10:04f2a82cfd89 | 49 | void set_parameter(float Kp, float Ki, float Kd) { |
echo_piyo | 10:04f2a82cfd89 | 50 | kp = Kp; |
echo_piyo | 10:04f2a82cfd89 | 51 | ki = Ki; |
echo_piyo | 10:04f2a82cfd89 | 52 | kd = Kd; |
echo_piyo | 10:04f2a82cfd89 | 53 | } |
echo_piyo | 10:04f2a82cfd89 | 54 | |
echo_piyo | 10:04f2a82cfd89 | 55 | float duty(){ |
echo_piyo | 10:04f2a82cfd89 | 56 | return result; |
echo_piyo | 10:04f2a82cfd89 | 57 | } |
echo_piyo | 10:04f2a82cfd89 | 58 | |
echo_piyo | 10:04f2a82cfd89 | 59 | float e,p,i,d; |
echo_piyo | 10:04f2a82cfd89 | 60 | |
echo_piyo | 10:04f2a82cfd89 | 61 | protected : |
echo_piyo | 10:04f2a82cfd89 | 62 | float kp,ki,kd; |
echo_piyo | 10:04f2a82cfd89 | 63 | float e1,e2; |
echo_piyo | 10:04f2a82cfd89 | 64 | //float p,i,d; |
echo_piyo | 10:04f2a82cfd89 | 65 | float result; |
echo_piyo | 10:04f2a82cfd89 | 66 | |
echo_piyo | 10:04f2a82cfd89 | 67 | void pid_cal(float target, float val, float dt){ |
echo_piyo | 10:04f2a82cfd89 | 68 | e2 = e1; |
echo_piyo | 10:04f2a82cfd89 | 69 | e1 = e; |
echo_piyo | 10:04f2a82cfd89 | 70 | e = target - val; |
echo_piyo | 10:04f2a82cfd89 | 71 | p = e - e1; |
echo_piyo | 10:04f2a82cfd89 | 72 | i = e; |
echo_piyo | 10:04f2a82cfd89 | 73 | d = (e-2*e1+e2); |
echo_piyo | 10:04f2a82cfd89 | 74 | result = result + (kp*p+ki*i+kd*d); |
echo_piyo | 10:04f2a82cfd89 | 75 | if (result > 1.0f) { |
echo_piyo | 10:04f2a82cfd89 | 76 | result = 1.0f; |
echo_piyo | 10:04f2a82cfd89 | 77 | } else if (result < -1.0f) { |
echo_piyo | 10:04f2a82cfd89 | 78 | result = -1.0f; |
echo_piyo | 10:04f2a82cfd89 | 79 | } |
echo_piyo | 10:04f2a82cfd89 | 80 | if ((target == 0) && (fabs(e) < invalidN)) { |
echo_piyo | 10:04f2a82cfd89 | 81 | result = 0; |
echo_piyo | 10:04f2a82cfd89 | 82 | } |
echo_piyo | 10:04f2a82cfd89 | 83 | } |
echo_piyo | 10:04f2a82cfd89 | 84 | }; |
echo_piyo | 10:04f2a82cfd89 | 85 | */ |
echo_piyo | 10:04f2a82cfd89 | 86 | class Encoder : public Position_pid_enc //: public Speed_pid |
echo_piyo | 10:04f2a82cfd89 | 87 | { |
echo_piyo | 10:04f2a82cfd89 | 88 | public: |
echo_piyo | 10:04f2a82cfd89 | 89 | Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) { |
echo_piyo | 10:04f2a82cfd89 | 90 | } |
echo_piyo | 10:04f2a82cfd89 | 91 | |
echo_piyo | 10:04f2a82cfd89 | 92 | void setup(int Ppr) { |
echo_piyo | 10:04f2a82cfd89 | 93 | ppr = Ppr; |
echo_piyo | 10:04f2a82cfd89 | 94 | A.rise(this, &Encoder::A_count); |
echo_piyo | 10:04f2a82cfd89 | 95 | B.rise(this, &Encoder::B_count); |
echo_piyo | 10:04f2a82cfd89 | 96 | origin(); |
echo_piyo | 10:04f2a82cfd89 | 97 | } |
echo_piyo | 10:04f2a82cfd89 | 98 | |
echo_piyo | 31:285c9898da03 | 99 | void cal(float target, float dt){ |
echo_piyo | 10:04f2a82cfd89 | 100 | radian = (float) count/ppr*2*PI; |
echo_piyo | 31:285c9898da03 | 101 | degree = radian / PI * 180.0; |
echo_piyo | 10:04f2a82cfd89 | 102 | w = (radian - oldtheta) / dt; |
echo_piyo | 10:04f2a82cfd89 | 103 | v = 0.05*w/27; |
echo_piyo | 10:04f2a82cfd89 | 104 | n = w*30/(PI); |
echo_piyo | 10:04f2a82cfd89 | 105 | oldtheta = radian; |
echo_piyo | 10:04f2a82cfd89 | 106 | //pid_cal(target, n/Nmax, dt); |
echo_piyo | 31:285c9898da03 | 107 | //deg_cylinder = (count / 1200) * 360 * 3 / 20; |
echo_piyo | 32:f535ace7c529 | 108 | position_cal(target, (float)degree, dt); |
echo_piyo | 31:285c9898da03 | 109 | } |
echo_piyo | 31:285c9898da03 | 110 | |
echo_piyo | 31:285c9898da03 | 111 | float deg(){ |
echo_piyo | 31:285c9898da03 | 112 | return degree; |
echo_piyo | 10:04f2a82cfd89 | 113 | } |
echo_piyo | 10:04f2a82cfd89 | 114 | |
echo_piyo | 31:285c9898da03 | 115 | float get_deg_cylinder(){ |
echo_piyo | 31:285c9898da03 | 116 | return deg_cylinder; |
echo_piyo | 10:04f2a82cfd89 | 117 | } |
echo_piyo | 10:04f2a82cfd89 | 118 | |
echo_piyo | 10:04f2a82cfd89 | 119 | float rad() { |
echo_piyo | 10:04f2a82cfd89 | 120 | return radian; |
echo_piyo | 10:04f2a82cfd89 | 121 | } |
echo_piyo | 10:04f2a82cfd89 | 122 | |
echo_piyo | 33:64fd1bd83bac | 123 | int pulse(){ |
echo_piyo | 10:04f2a82cfd89 | 124 | return count; |
echo_piyo | 10:04f2a82cfd89 | 125 | } |
echo_piyo | 10:04f2a82cfd89 | 126 | |
echo_piyo | 10:04f2a82cfd89 | 127 | float N(){ |
echo_piyo | 10:04f2a82cfd89 | 128 | return n; |
echo_piyo | 10:04f2a82cfd89 | 129 | } |
echo_piyo | 10:04f2a82cfd89 | 130 | |
echo_piyo | 10:04f2a82cfd89 | 131 | void origin() { |
echo_piyo | 10:04f2a82cfd89 | 132 | count = 0; |
echo_piyo | 10:04f2a82cfd89 | 133 | } |
echo_piyo | 10:04f2a82cfd89 | 134 | |
echo_piyo | 10:04f2a82cfd89 | 135 | private : |
echo_piyo | 10:04f2a82cfd89 | 136 | InterruptIn A; |
echo_piyo | 10:04f2a82cfd89 | 137 | InterruptIn B; |
echo_piyo | 10:04f2a82cfd89 | 138 | |
echo_piyo | 10:04f2a82cfd89 | 139 | int ppr; |
echo_piyo | 33:64fd1bd83bac | 140 | signed int count; |
echo_piyo | 10:04f2a82cfd89 | 141 | float radian,oldtheta; |
echo_piyo | 10:04f2a82cfd89 | 142 | float w,v,n; |
echo_piyo | 10:04f2a82cfd89 | 143 | float degree, deg_cylinder; |
echo_piyo | 10:04f2a82cfd89 | 144 | |
echo_piyo | 10:04f2a82cfd89 | 145 | void A_count() { |
echo_piyo | 10:04f2a82cfd89 | 146 | count ++; |
echo_piyo | 10:04f2a82cfd89 | 147 | } |
echo_piyo | 10:04f2a82cfd89 | 148 | |
echo_piyo | 10:04f2a82cfd89 | 149 | void B_count() { |
echo_piyo | 10:04f2a82cfd89 | 150 | count --; |
echo_piyo | 10:04f2a82cfd89 | 151 | } |
echo_piyo | 10:04f2a82cfd89 | 152 | }; |