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@49:b041c815c063, 2017-09-13 (annotated)

Committer:
echo_piyo
Date:
Wed Sep 13 01:25:07 2017 +0000
Revision:
49:b041c815c063
Parent:
33:64fd1bd83bac 
(?)????????????????????????????????; (enc).duty_enableWidth(min,max)?????????????????????; ????????????????

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 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 };