File content as of revision 8:c09ae4fef420:

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


Polyservo::Polyservo(DigitalOut servo_pins[]) {
    servos.reserve(numservos); servo_timers.reserve(numservos); //positions.reserve(numservos);
    //numservos = num_pins; 
    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);
        }
        
    }
}    
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); }