PolyServo.cpp

00001 
00002 #include "PolyServo.h"
00003 #include "mbed.h"
00004 //#include "FPointer.h"
00005 #include <vector>
00006 //#include "SpecifiedTimeout.h"
00007 
00008 
00009 Polyservo::Polyservo(DigitalOut servo_pins[]) {
00010     numservos = NUM_SERVOS;
00011     servos.reserve(numservos); servo_timers.reserve(numservos); //positions.reserve(numservos);
00012     //numservos = num_pins; 
00013     servo_sets[0] = &Polyservo::servo0_set;
00014     servo_sets[1] = &Polyservo::servo1_set;
00015     servo_sets[2] = &Polyservo::servo2_set;
00016     servo_sets[3] = &Polyservo::servo3_set;
00017     servo_sets[4] = &Polyservo::servo4_set;
00018     servo_sets[5] = &Polyservo::servo5_set;
00019     servo_sets[6] = &Polyservo::servo6_set;
00020     servo_sets[7] = &Polyservo::servo7_set;
00021     servo_sets[8] = &Polyservo::servo8_set;
00022     servo_sets[9] = &Polyservo::servo9_set;
00023     servo_sets[10] = &Polyservo::servo10_set;
00024     servo_sets[11] = &Polyservo::servo11_set;
00025 
00026     
00027     for(int i = 0; i < numservos; i++){
00028         Timeout* t = new Timeout(); 
00029         servos.push_back(&servo_pins[i]);
00030         servo_timers.push_back(t);
00031         //sssstions.push_back(SERVO_DISABLED);
00032         positions[i] = SERVO_DISABLED;
00033         servos[i]->write(0);
00034                
00035     }
00036     
00037     //servos[2]->is_connected();
00038     //servo_timers[2]->detach();
00039     period_tick.attach_us(this, &Polyservo::servos_reset,PPM_PERIOD);
00040     
00041 }
00042 
00043 void Polyservo::servos_reset(void){
00044     for(int i=0; i< numservos; i++) {
00045         if (global_enable && positions[i] != SERVO_DISABLED){
00046             servos[i]->write(1);
00047             //ResetWrapper<void(*)(int), servo_set> rwrapper(i);
00048             servo_timers[i]->attach_us(this, &Polyservo::servo1_set, positions[i]);
00049 
00050         } else {
00051             servos[i]->write(0);
00052             servo_timers[i]->detach();
00053         }
00054         
00055     }
00056 }    
00057 void Polyservo::servo0_set(void) {  servos[0]->write(0); }
00058 void Polyservo::servo1_set(void) {  servos[1]->write(0); }
00059 void Polyservo::servo2_set(void) {  servos[2]->write(0); }
00060 void Polyservo::servo3_set(void) {  servos[3]->write(0); }
00061 void Polyservo::servo4_set(void) {  servos[4]->write(0); }
00062 void Polyservo::servo5_set(void) {  servos[5]->write(0); }
00063 void Polyservo::servo6_set(void) {  servos[6]->write(0); }
00064 void Polyservo::servo7_set(void) {  servos[7]->write(0); }
00065 void Polyservo::servo8_set(void) {  servos[8]->write(0); }
00066 void Polyservo::servo9_set(void) {  servos[9]->write(0); }
00067 void Polyservo::servo10_set(void) {  servos[10]->write(0); }
00068 void Polyservo::servo11_set(void) {  servos[11]->write(0); }
00069 //void Polyservo::servo12_set(void) {  servos[0]->write(0); }
00070 
00071     
00072     
00073     
00074 
00075     
00076     
00077 
00078     
00079