File content as of revision 7:f9e1e813fa24:

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


Polyservo::Polyservo(DigitalOut servo_pins[], int num_pins): numservos(num_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);
        positions.push_back(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(&(rwrapper->f), positions[i]);
        } else {
            servos[i]->write(0);
        }
        
    }
}    
void servo_set(int id) {
    
    
    
}