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
encorder.h@31:285c9898da03, 2017-08-29 (annotated)
- Committer:
- echo_piyo
- Date:
- Tue Aug 29 02:07:56 2017 +0000
- Revision:
- 31:285c9898da03
- Parent:
- 10:04f2a82cfd89
- Child:
- 32:f535ace7c529
(?)??????????;
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 | 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 | 31:285c9898da03 | 90 | void cal(float target, float dt){ |
echo_piyo | 10:04f2a82cfd89 | 91 | radian = (float) count/ppr*2*PI; |
echo_piyo | 31:285c9898da03 | 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 | 31:285c9898da03 | 98 | //deg_cylinder = (count / 1200) * 360 * 3 / 20; |
echo_piyo | 31:285c9898da03 | 99 | position_cal(target, (float)count, dt); |
echo_piyo | 31:285c9898da03 | 100 | } |
echo_piyo | 31:285c9898da03 | 101 | |
echo_piyo | 31:285c9898da03 | 102 | float deg(){ |
echo_piyo | 31:285c9898da03 | 103 | return degree; |
echo_piyo | 10:04f2a82cfd89 | 104 | } |
echo_piyo | 10:04f2a82cfd89 | 105 | |
echo_piyo | 31:285c9898da03 | 106 | float get_deg_cylinder(){ |
echo_piyo | 31:285c9898da03 | 107 | return deg_cylinder; |
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 | }; |