![](/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
encorder.h
- Committer:
- echo_piyo
- Date:
- 2017-09-13
- Revision:
- 49:b041c815c063
- Parent:
- 33:64fd1bd83bac
File content as of revision 49:b041c815c063:
#define PI 3.141592654f #define Nmax 18000 #define invalidN 0.1 class Position_pid_enc { public : void set_parameter(float Kp, float Ki, float Kd) { kp = Kp; ki = Ki; kd = Kd; } void position_cal(float target, float nowval, float dt) { old = now; now = nowval - target; p = now; i = i + (now + old)/2.0f * dt; d = (now - old) / dt; result = kp*p + ki*i + kd*d; if (result > 1.0f) { result = 1.0f; } else if (result < -1.0f) { result = -1.0f; } } float duty() { return result; } float duty_enableWidth(float duty_min, float duty_max){ if(now > duty_min && now < duty_max){ //nowは差 return 0.0; }else{ return result; } } private : float kp, ki, kd, old, now, p, i, d, result, duty_min, duty_max; }; /* class Speed_pid { public: void set_parameter(float Kp, float Ki, float Kd) { kp = Kp; ki = Ki; kd = Kd; } float duty(){ return result; } float e,p,i,d; protected : float kp,ki,kd; float e1,e2; //float p,i,d; float result; void pid_cal(float target, float val, float dt){ e2 = e1; e1 = e; e = target - val; p = e - e1; i = e; d = (e-2*e1+e2); result = result + (kp*p+ki*i+kd*d); if (result > 1.0f) { result = 1.0f; } else if (result < -1.0f) { result = -1.0f; } if ((target == 0) && (fabs(e) < invalidN)) { result = 0; } } }; */ class Encoder : public Position_pid_enc //: public Speed_pid { public: Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) { } void setup(int Ppr) { ppr = Ppr; A.rise(this, &Encoder::A_count); B.rise(this, &Encoder::B_count); origin(); } void cal(float target, float dt){ radian = (float) count/ppr*2*PI; degree = radian / PI * 180.0; w = (radian - oldtheta) / dt; v = 0.05*w/27; n = w*30/(PI); oldtheta = radian; //pid_cal(target, n/Nmax, dt); //deg_cylinder = (count / 1200) * 360 * 3 / 20; position_cal(target, (float)degree, dt); } float deg(){ return degree; } float get_deg_cylinder(){ return deg_cylinder; } float rad() { return radian; } int pulse(){ return count; } float N(){ return n; } void origin() { count = 0; } private : InterruptIn A; InterruptIn B; int ppr; signed int count; float radian,oldtheta; float w,v,n; float degree, deg_cylinder; void A_count() { count ++; } void B_count() { count --; } };