rotaryinc.cpp

00001 #include "rotaryinc.hpp"
00002 
00003 rotaryinc::rotaryinc(PinName pinA,PinName pinB,double diameter,double resolution,double dt)
00004 {
00005     Diameter = diameter;
00006     Resolution = resolution;
00007     dt_ = dt;
00008     init(pinA,pinB);
00009     pulse[0] = 0;
00010     pulse[1] = 0;
00011     speed = 0;
00012     timer.start();
00013 }
00014 
00015 void rotaryinc::init(PinName pin_a,PinName pin_b)
00016 {
00017     pin_a_ = new InterruptIn(pin_a,PullUp);
00018     pin_b_ = new InterruptIn(pin_b,PullUp);
00019     pin_a_->rise(callback(this,&rotaryinc::riseA));
00020     pin_a_->fall(callback(this,&rotaryinc::fallA));
00021     pin_b_->rise(callback(this,&rotaryinc::riseB));
00022     pin_b_->fall(callback(this,&rotaryinc::fallB));
00023 }
00024 
00025 void rotaryinc::riseA()
00026 {
00027     pin_b_ -> read() ? pulse[1]-- : pulse[1]++;
00028 }
00029 
00030 void rotaryinc::fallA()
00031 {
00032     pin_b_ -> read() ? pulse[1]++ : pulse[1]--;
00033 }
00034 
00035 void rotaryinc::riseB()
00036 {
00037     pin_a_ -> read() ? pulse[1]++ : pulse[1]--;
00038 }
00039 
00040 void rotaryinc::fallB()
00041 {
00042     pin_a_ -> read() ? pulse[1]-- : pulse[1]++;
00043 }
00044 
00045 double rotaryinc::getpulse()
00046 {
00047     return pulse[1]/4;
00048 }
00049 
00050 double rotaryinc::getspeed()
00051 {
00052     speed = (M_PI * Diameter) * (pulse[1]/4 - pulse[0]) / Resolution / dt_;
00053     pulse[0] = pulse[1]/4;
00054     return speed;
00055 }
00056 
00057 double rotaryinc::gettheta()
00058 {
00059     theta[1] = pulse[1]/4 / Resolution * 360;
00060     return theta[1];
00061 }
00062 
00063 double rotaryinc::getomega()
00064 {
00065     omega = (theta[1] - theta[0]) / dt_;
00066     theta[0] = theta[1];
00067     return omega;
00068 }
00069 
00070 double rotaryinc::getrevolution()
00071 {
00072     return pulse[1]/4 / Resolution;
00073 }
00074 
00075 void rotaryinc::loop()
00076 {
00077     time = timer.read();
00078     while(timer.read() - time <= dt_);
00079 }
00080 
00081 rotaryinc::~rotaryinc(){
00082     pin_a_->disable_irq();
00083     pin_b_->disable_irq();
00084     delete pin_a_;
00085     delete pin_b_;
00086 }