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