ServoIn.cpp

00001 // Uses Timer to read servo pulse on single input pin
00002 // J. Bradshaw 20140925
00003 /* Copyright (c) 2015, jbradshaw (http://mbed.org)
00004  *
00005  * Permission is hereby granted, free of charge, to any person obtaining a copy
00006  * of this software and associated documentation files (the "Software"), to deal
00007  * in the Software without restriction, including without limitation the rights
00008  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00009  * copies of the Software, and to permit persons to whom the Software is
00010  * furnished to do so, subject to the following conditions:
00011  *
00012  * The above copyright notice and this permission notice shall be included in
00013  * all copies or substantial portions of the Software.
00014  *
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00016  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00017  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00018  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00019  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00021  * THE SOFTWARE.
00022  *
00023 */ 
00024 #include "mbed.h"
00025 #include "ServoIn.h"
00026 
00027 ServoIn::ServoIn(PinName pin): _interrupt(pin), _pin(pin){        // create the InterruptIn on the pin specified to ServoIn
00028     _interrupt.rise(this, &ServoIn::PulseRead); // attach PulseRead function of this ServoIn instance
00029     
00030     _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015); // the member function, and interval (15ms)
00031     pulseMeasure.start();
00032     if(_pin)
00033         _pulseFlag = 1;
00034     else
00035         _pulseFlag = 0;
00036     servoPulse = 0;
00037     servoPulseMax = 2100;
00038     servoPulseMin = 900;
00039     _t_state=0;    
00040 }
00041 
00042 //function to read the servo pulse duration
00043 void ServoIn::PulseRead() {
00044     if(!_pulseFlag){ //if the flag is low, was waiting for high input interrupt
00045         this->pulseMeasure.reset();   //reset the timer
00046         _interrupt.fall(this, &ServoIn::PulseRead);
00047         _pulseFlag = 1;  //set the pulse input flag
00048     }
00049     else{
00050         servoPulse = pulseMeasure.read_us();
00051         if(servoPulse < this->servoPulseMax){
00052             _interrupt.rise(this, &ServoIn::PulseRead);
00053             _pulseFlag = 0;
00054         }
00055         else{
00056             _pulseFlag = 1;  //set the pulse input flag
00057         }
00058     }
00059     _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015); //setup interrupt for 15ms
00060     _t_state=0;  //re-zero t-state    
00061 }
00062 
00063 
00064 int ServoIn::read(void){
00065     return servoPulse;    
00066 }
00067 
00068 float ServoIn::readCalPulse(void){
00069     servoPulseOffset = ((float)servoPulseMax + (float)servoPulseMin) / 2.0;
00070     servoPulseScale = 1.0 / (((float)servoPulseMax - (float)servoPulseMin) / 2.0);
00071     
00072     float pulseCal = ((float)servoPulse-servoPulseOffset)*servoPulseScale;
00073     if((pulseCal > 1.5) || (pulseCal < -1.5))
00074         return 0.0;
00075         
00076     return ((float)servoPulse-servoPulseOffset)*servoPulseScale;    
00077 }
00078 
00079 void ServoIn::timeoutTest(void){
00080     if(!_t_state){
00081         _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015);
00082         _t_state=1;  //toggle t_sate (15ms has elapsed without servo edge
00083     }
00084     else{   //30 ms has elapsed without a valid servo pulse
00085         _pulseFlag = 0;
00086         _t_state=0;
00087         servoPulse = 0;
00088         _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015);
00089     }
00090 }
00091