kusano kiyoshige / pid_encoder

first

Dependents:   17robo_fuzi 17robo_tokyo_kaede

pid_encoder.cpp@0:929eee8e2c2c, 2017-09-24 (annotated)

Committer:
echo_piyo
Date:
Sun Sep 24 05:24:06 2017 +0000
Revision:
0:929eee8e2c2c
????2??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:929eee8e2c2c 1 #include "pid_encoder.h"
echo_piyo 0:929eee8e2c2c 2
echo_piyo 0:929eee8e2c2c 3 Encoder::Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) {
echo_piyo 0:929eee8e2c2c 4 }
echo_piyo 0:929eee8e2c2c 5
echo_piyo 0:929eee8e2c2c 6 void Encoder::setPpr(int Ppr){
echo_piyo 0:929eee8e2c2c 7 ppr = Ppr;
echo_piyo 0:929eee8e2c2c 8 A.rise(this, &Encoder::A_count);
echo_piyo 0:929eee8e2c2c 9 B.rise(this, &Encoder::B_count);
echo_piyo 0:929eee8e2c2c 10 origin();
echo_piyo 0:929eee8e2c2c 11 }
echo_piyo 0:929eee8e2c2c 12
echo_piyo 0:929eee8e2c2c 13 void Encoder::cal(float target, float dt){
echo_piyo 0:929eee8e2c2c 14 radian = (float) count/ppr*2*PI;
echo_piyo 0:929eee8e2c2c 15 degree = radian / PI * 180.0;
echo_piyo 0:929eee8e2c2c 16 w = (radian - oldtheta) / dt;
echo_piyo 0:929eee8e2c2c 17 v = 0.05*w/27;
echo_piyo 0:929eee8e2c2c 18 n = w*30/(PI);
echo_piyo 0:929eee8e2c2c 19 oldtheta = radian;
echo_piyo 0:929eee8e2c2c 20 //pid_cal(target, n/Nmax, dt);
echo_piyo 0:929eee8e2c2c 21 //deg_cylinder = (count / 1200) * 360 * 3 / 20;
echo_piyo 0:929eee8e2c2c 22 calculate(target, (float)degree);
echo_piyo 0:929eee8e2c2c 23 }
echo_piyo 0:929eee8e2c2c 24
echo_piyo 0:929eee8e2c2c 25 float Encoder::deg(){
echo_piyo 0:929eee8e2c2c 26 return degree;
echo_piyo 0:929eee8e2c2c 27 }
echo_piyo 0:929eee8e2c2c 28
echo_piyo 0:929eee8e2c2c 29 float Encoder::get_deg_cylinder(){
echo_piyo 0:929eee8e2c2c 30 return deg_cylinder;
echo_piyo 0:929eee8e2c2c 31 }
echo_piyo 0:929eee8e2c2c 32
echo_piyo 0:929eee8e2c2c 33 float Encoder::rad(){
echo_piyo 0:929eee8e2c2c 34 return radian;
echo_piyo 0:929eee8e2c2c 35 }
echo_piyo 0:929eee8e2c2c 36
echo_piyo 0:929eee8e2c2c 37 int Encoder::pulse(){
echo_piyo 0:929eee8e2c2c 38 return count;
echo_piyo 0:929eee8e2c2c 39 }
echo_piyo 0:929eee8e2c2c 40
echo_piyo 0:929eee8e2c2c 41 float Encoder::N(){
echo_piyo 0:929eee8e2c2c 42 return n;
echo_piyo 0:929eee8e2c2c 43 }
echo_piyo 0:929eee8e2c2c 44
echo_piyo 0:929eee8e2c2c 45 void Encoder::origin(){
echo_piyo 0:929eee8e2c2c 46 count = 0;
echo_piyo 0:929eee8e2c2c 47 }
echo_piyo 0:929eee8e2c2c 48
echo_piyo 0:929eee8e2c2c 49 void Encoder::A_count(){
echo_piyo 0:929eee8e2c2c 50 count ++;
echo_piyo 0:929eee8e2c2c 51 }
echo_piyo 0:929eee8e2c2c 52
echo_piyo 0:929eee8e2c2c 53 void Encoder::B_count(){
echo_piyo 0:929eee8e2c2c 54 count --;
echo_piyo 0:929eee8e2c2c 55 }