足回り動かすためのライブラリ
Embed:
(wiki syntax)
Show/hide line numbers
encoder.cpp
00001 #include "encoder.hpp" 00002 00003 Encoder::Encoder(PinName pinA,PinName pinB,int resolution,int mode,int id):resolution(resolution),mode(mode),ID(id){ 00004 pin_A = new InterruptIn(pinA,PullUp); 00005 pin_B = new InterruptIn(pinB,PullUp); 00006 pulse = 0; 00007 theta[0] = 0.0; 00008 theta[1] = 0.0; 00009 omega[0] = 0.0; 00010 omega[1] = 0.0; 00011 alpha[0] = 0.0; 00012 alpha[1] = 0.0; 00013 jerk = 0.0; 00014 init(); 00015 } 00016 Encoder::~Encoder(){ 00017 pin_B->disable_irq(); 00018 pin_B->disable_irq(); 00019 delete pin_A; 00020 delete pin_B; 00021 } 00022 00023 void Encoder::Update(double dt){ 00024 theta[1] = 2.0*M_PI * pulse / resolution / mode; 00025 omega[1] = (theta[1]-theta[0])/dt; 00026 alpha[1] = (omega[1]-omega[0])/dt; 00027 jerk = (alpha[1]-alpha[0])/dt; 00028 theta[0] = theta[1]; 00029 omega[0] = omega[1]; 00030 alpha[0] = alpha[1]; 00031 } 00032 00033 double Encoder::get_Theta() {return theta[1];} 00034 double Encoder::get_Omega() {return omega[1];} 00035 double Encoder::get_Alpha() {return alpha[1];} 00036 double Encoder::get_Jerk() {return jerk;} 00037 double Encoder::get_ID() {return ID;} 00038 00039 void Encoder::init(){ 00040 pulse = 0; 00041 if(mode <= 0 or 4 < mode) mode = 4; 00042 if(1 <= mode) pin_A->rise(callback(this,&Encoder::riseA)); 00043 if(2 <= mode) pin_A->fall(callback(this,&Encoder::fallA)); 00044 if(3 <= mode) pin_B->rise(callback(this,&Encoder::riseB)); 00045 if(4 <= mode) pin_B->fall(callback(this,&Encoder::fallB)); 00046 } 00047 00048 void Encoder::riseA(void){pin_B->read() ? pulse-- : pulse++;} 00049 void Encoder::riseB(void){pin_A->read() ? pulse++ : pulse--;} 00050 void Encoder::fallA(void){pin_B->read() ? pulse++ : pulse--;} 00051 void Encoder::fallB(void){pin_A->read() ? pulse-- : pulse++;}
Generated on Thu Jul 14 2022 14:37:03 by 1.7.2