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());        
    }
}

Diff: ServoIn.cpp

Revision:
3:19c8eaf905e9
Parent:
1:b7cc6da72d09
Child:
4:379f9ab5cb4b
diff -r 07273d6016e9 -r 19c8eaf905e9 ServoIn.cpp
--- a/ServoIn.cpp	Sat May 09 15:25:03 2015 +0000
+++ b/ServoIn.cpp	Thu May 21 18:40:06 2015 +0000
@@ -31,7 +31,9 @@
     pulseMeasure.start();
     _pulseFlag = 0;
     servoPulse = 0;
-    _t_state=0;
+    servoPulseMax = 2100;
+    servoPulseMin = 900;
+    _t_state=0;    
 }
 
 //function to read the servo pulse duration
@@ -55,6 +57,17 @@
     return servoPulse;    
 }
 
+float ServoIn::readCalPulse(void){
+    servoPulseOffset = ((float)servoPulseMax + (float)servoPulseMin) / 2.0;
+    servoPulseScale = 1.0 / (((float)servoPulseMax - (float)servoPulseMin) / 2.0);
+    
+    float pulseCal = ((float)servoPulse-servoPulseOffset)*servoPulseScale;
+    if((pulseCal > 1.5) || (pulseCal < -1.5))
+        return 0.0;
+        
+    return ((float)servoPulse-servoPulseOffset)*servoPulseScale;    
+}
+
 void ServoIn::timeoutTest(void){
     if(!_t_state){
         _validPulseTimeout.attach(this, &ServoIn::timeoutTest, 0.015);