Joseph Bradshaw / ServoIn

Class Library for reading hobby servos and detecting invalid or disconnected channels

Dependents:   MadPulseCntrl Emaxx_Navigation Emaxx_Navigation_Dynamic_HIL Madpulse_Speed_Control_temp ... more

ServoIn Class Library

This class library uses external interrupts with timer functions to read servos pulses on mbed input pins.

ServoIn Library usage example

#include "mbed.h"
#include "ServoIn.h"

DigitalOut led1(LED1);
ServoIn servoIn1(p15);
ServoIn servoIn2(p14);
Serial pc(USBTX, USBRX); // tx, rx

int main() {    
    pc.baud(921600);            //Fast baud rate
        
    while(1) {
        led1 = 1;
        wait(0.05);
        led1 = 0;
        wait(0.05);
        
        pc.printf("servo pulse: CH1=%5dus  CH2=%5dus\r\n", servoIn1.read(), servoIn2.read());        
    }
}

ServoIn.cpp@5:598ccee73ed3, 2017-08-14 (annotated)

Committer:
jebradshaw
Date:
Mon Aug 14 14:51:20 2017 +0000
Revision:
5:598ccee73ed3
Parent:
4:379f9ab5cb4b 
added check for valid servo pulse when greater then servo max to prevent flag check after pulse interrupt (inverted pulse measure phase)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:98a3b6fbd242 1 // Uses Timer to read servo pulse on single input pin
jebradshaw 0:98a3b6fbd242 2 // J. Bradshaw 20140925
jebradshaw 0:98a3b6fbd242 3 /* Copyright (c) 2015, jbradshaw (http://mbed.org)
jebradshaw 0:98a3b6fbd242 4 *
jebradshaw 0:98a3b6fbd242 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
jebradshaw 0:98a3b6fbd242 6 * of this software and associated documentation files (the "Software"), to deal
jebradshaw 0:98a3b6fbd242 7 * in the Software without restriction, including without limitation the rights
jebradshaw 0:98a3b6fbd242 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jebradshaw 0:98a3b6fbd242 9 * copies of the Software, and to permit persons to whom the Software is
jebradshaw 0:98a3b6fbd242 10 * furnished to do so, subject to the following conditions:
jebradshaw 0:98a3b6fbd242 11 *
jebradshaw 0:98a3b6fbd242 12 * The above copyright notice and this permission notice shall be included in
jebradshaw 0:98a3b6fbd242 13 * all copies or substantial portions of the Software.
jebradshaw 0:98a3b6fbd242 14 *
jebradshaw 0:98a3b6fbd242 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jebradshaw 0:98a3b6fbd242 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jebradshaw 0:98a3b6fbd242 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jebradshaw 0:98a3b6fbd242 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jebradshaw 0:98a3b6fbd242 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jebradshaw 0:98a3b6fbd242 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jebradshaw 0:98a3b6fbd242 21 * THE SOFTWARE.
jebradshaw 0:98a3b6fbd242 22 *
jebradshaw 0:98a3b6fbd242 23 */
jebradshaw 0:98a3b6fbd242 24 #include "mbed.h"
jebradshaw 0:98a3b6fbd242 25 #include "ServoIn.h"
jebradshaw 0:98a3b6fbd242 26
jebradshaw 4:379f9ab5cb4b 27 ServoIn::ServoIn(PinName pin): _interrupt(pin), _pin(pin){ // create the InterruptIn on the pin specified to ServoIn
jebradshaw 0:98a3b6fbd242 28 _interrupt.rise(this, &ServoIn::PulseRead); // attach PulseRead function of this ServoIn instance
jebradshaw 4:379f9ab5cb4b 29
jebradshaw 1:b7cc6da72d09 30 _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015); // the member function, and interval (15ms)
jebradshaw 0:98a3b6fbd242 31 pulseMeasure.start();
jebradshaw 4:379f9ab5cb4b 32 if(_pin)
jebradshaw 4:379f9ab5cb4b 33 _pulseFlag = 1;
jebradshaw 4:379f9ab5cb4b 34 else
jebradshaw 4:379f9ab5cb4b 35 _pulseFlag = 0;
jebradshaw 0:98a3b6fbd242 36 servoPulse = 0;
jebradshaw 3:19c8eaf905e9 37 servoPulseMax = 2100;
jebradshaw 3:19c8eaf905e9 38 servoPulseMin = 900;
jebradshaw 3:19c8eaf905e9 39 _t_state=0;
jebradshaw 0:98a3b6fbd242 40 }
jebradshaw 0:98a3b6fbd242 41
jebradshaw 0:98a3b6fbd242 42 //function to read the servo pulse duration
jebradshaw 0:98a3b6fbd242 43 void ServoIn::PulseRead() {
jebradshaw 0:98a3b6fbd242 44 if(!_pulseFlag){ //if the flag is low, was waiting for high input interrupt
jebradshaw 0:98a3b6fbd242 45 this->pulseMeasure.reset(); //reset the timer
jebradshaw 0:98a3b6fbd242 46 _interrupt.fall(this, &ServoIn::PulseRead);
jebradshaw 0:98a3b6fbd242 47 _pulseFlag = 1; //set the pulse input flag
jebradshaw 0:98a3b6fbd242 48 }
jebradshaw 0:98a3b6fbd242 49 else{
jebradshaw 0:98a3b6fbd242 50 servoPulse = pulseMeasure.read_us();
jebradshaw 5:598ccee73ed3 51 if(servoPulse < this->servoPulseMax){
jebradshaw 5:598ccee73ed3 52 _interrupt.rise(this, &ServoIn::PulseRead);
jebradshaw 5:598ccee73ed3 53 _pulseFlag = 0;
jebradshaw 5:598ccee73ed3 54 }
jebradshaw 5:598ccee73ed3 55 else{
jebradshaw 5:598ccee73ed3 56 _pulseFlag = 1; //set the pulse input flag
jebradshaw 5:598ccee73ed3 57 }
jebradshaw 1:b7cc6da72d09 58 }
jebradshaw 1:b7cc6da72d09 59 _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015); //setup interrupt for 15ms
jebradshaw 1:b7cc6da72d09 60 _t_state=0; //re-zero t-state
jebradshaw 0:98a3b6fbd242 61 }
jebradshaw 0:98a3b6fbd242 62
jebradshaw 0:98a3b6fbd242 63
jebradshaw 0:98a3b6fbd242 64 int ServoIn::read(void){
jebradshaw 0:98a3b6fbd242 65 return servoPulse;
jebradshaw 1:b7cc6da72d09 66 }
jebradshaw 1:b7cc6da72d09 67
jebradshaw 3:19c8eaf905e9 68 float ServoIn::readCalPulse(void){
jebradshaw 3:19c8eaf905e9 69 servoPulseOffset = ((float)servoPulseMax + (float)servoPulseMin) / 2.0;
jebradshaw 3:19c8eaf905e9 70 servoPulseScale = 1.0 / (((float)servoPulseMax - (float)servoPulseMin) / 2.0);
jebradshaw 3:19c8eaf905e9 71
jebradshaw 3:19c8eaf905e9 72 float pulseCal = ((float)servoPulse-servoPulseOffset)*servoPulseScale;
jebradshaw 3:19c8eaf905e9 73 if((pulseCal > 1.5) || (pulseCal < -1.5))
jebradshaw 3:19c8eaf905e9 74 return 0.0;
jebradshaw 3:19c8eaf905e9 75
jebradshaw 3:19c8eaf905e9 76 return ((float)servoPulse-servoPulseOffset)*servoPulseScale;
jebradshaw 3:19c8eaf905e9 77 }
jebradshaw 3:19c8eaf905e9 78
jebradshaw 1:b7cc6da72d09 79 void ServoIn::timeoutTest(void){
jebradshaw 1:b7cc6da72d09 80 if(!_t_state){
jebradshaw 1:b7cc6da72d09 81 _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015);
jebradshaw 1:b7cc6da72d09 82 _t_state=1; //toggle t_sate (15ms has elapsed without servo edge
jebradshaw 1:b7cc6da72d09 83 }
jebradshaw 1:b7cc6da72d09 84 else{ //30 ms has elapsed without a valid servo pulse
jebradshaw 1:b7cc6da72d09 85 _pulseFlag = 0;
jebradshaw 1:b7cc6da72d09 86 _t_state=0;
jebradshaw 1:b7cc6da72d09 87 servoPulse = 0;
jebradshaw 1:b7cc6da72d09 88 _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015);
jebradshaw 1:b7cc6da72d09 89 }
jebradshaw 1:b7cc6da72d09 90 }
jebradshaw 1:b7cc6da72d09 91