File content as of revision 10:7d3d0d016758:

#include "PolyServo.h"
#include "mbed.h"
//#include "FPointer.h"
#include <vector>
//#include "SpecifiedTimeout.h"


Polyservo::Polyservo(DigitalOut servo_pins[]) {
    numservos = NUM_SERVOS;
    servos.reserve(numservos); servo_timers.reserve(numservos); //positions.reserve(numservos);
    //numservos = num_pins; 
    servo_sets[0] = &Polyservo::servo0_set;
    servo_sets[1] = &Polyservo::servo1_set;
    servo_sets[2] = &Polyservo::servo2_set;
    servo_sets[3] = &Polyservo::servo3_set;
    servo_sets[4] = &Polyservo::servo4_set;
    servo_sets[5] = &Polyservo::servo5_set;
    servo_sets[6] = &Polyservo::servo6_set;
    servo_sets[7] = &Polyservo::servo7_set;
    servo_sets[8] = &Polyservo::servo8_set;
    servo_sets[9] = &Polyservo::servo9_set;
    servo_sets[10] = &Polyservo::servo10_set;
    servo_sets[11] = &Polyservo::servo11_set;

    
    for(int i = 0; i < numservos; i++){
        Timeout* t = new Timeout(); 
        servos.push_back(&servo_pins[i]);
        servo_timers.push_back(t);
        //sssstions.push_back(SERVO_DISABLED);
        positions[i] = SERVO_DISABLED;
        servos[i]->write(0);
               
    }
    
    //servos[2]->is_connected();
    //servo_timers[2]->detach();
    period_tick.attach_us(this, &Polyservo::servos_reset,PPM_PERIOD);
    
}

void Polyservo::servos_reset(void){
    for(int i=0; i< numservos; i++) {
        if (global_enable && positions[i] != SERVO_DISABLED){
            servos[i]->write(1);
            //ResetWrapper<void(*)(int), servo_set> rwrapper(i);
            servo_timers[i]->attach_us(this, &Polyservo::servo1_set, positions[i]);

        } else {
            servos[i]->write(0);
            servo_timers[i]->detach();
        }
        
    }
}    
void Polyservo::servo0_set(void) {  servos[0]->write(0); }
void Polyservo::servo1_set(void) {  servos[1]->write(0); }
void Polyservo::servo2_set(void) {  servos[2]->write(0); }
void Polyservo::servo3_set(void) {  servos[3]->write(0); }
void Polyservo::servo4_set(void) {  servos[4]->write(0); }
void Polyservo::servo5_set(void) {  servos[5]->write(0); }
void Polyservo::servo6_set(void) {  servos[6]->write(0); }
void Polyservo::servo7_set(void) {  servos[7]->write(0); }
void Polyservo::servo8_set(void) {  servos[8]->write(0); }
void Polyservo::servo9_set(void) {  servos[9]->write(0); }
void Polyservo::servo10_set(void) {  servos[10]->write(0); }
void Polyservo::servo11_set(void) {  servos[11]->write(0); }
//void Polyservo::servo12_set(void) {  servos[0]->write(0); }