PwmOut myServo1(PB_3); definir puertos de los servos PwmOut myServo2(PB_4); PwmOut myServo3(PB_5); PwmOut myServo4(PB_8); PwmOut myServo5(PB_9); PwmOut myServo6(PA_5); PwmOut myServo7(PA_6); PwmOut myServo8(PC_8);

uint8_t ss_time=50; tiempo de espera para moverse 1 mm en microsegundos

void put_sstime(uint8_t vtime){ ss_time=vtime; cargar tiempo

}

int coord2us(float coord) { if(0 <= coord <= MAXPOS) return int(750+coord*1900/50); cambiar de numero de entrada a grados return 750;

}

void mover_ser(uint8_t nmotor, uint8_t grados){ definimos que leen los 8 servos y le asiganamos a cada variable un servo

int pulsea = coord2us(grados); int pulseb = coord2us(grados); int pulsec = coord2us(grados); int pulsed = coord2us(grados); int pulsee = coord2us(grados); int pulsef = coord2us(grados); int pulseg = coord2us(grados); int pulseh = coord2us(grados); switch (nmotor) { case 0x01: myServo1.pulsewidth_us(pulsea);break; caso para cada servo case 0x02: myServo2.pulsewidth_us(pulseb);break; case 0x03: myServo2.pulsewidth_us(pulsec);break; case 0x04: myServo2.pulsewidth_us(pulsed);break; case 0x05: myServo2.pulsewidth_us(pulsee);break; case 0x06: myServo2.pulsewidth_us(pulsef);break; case 0x07: myServo2.pulsewidth_us(pulseg);break; case 0x08: myServo2.pulsewidth_us(pulseh);break; default: debug_m("numero de motor no valido");break ;

} }

void init_servo() { myServo1.period_ms(20); periodo de cada servo myServo2.period_ms(20); myServo3.period_ms(20); myServo4.period_ms(20); myServo5.period_ms(20); myServo6.period_ms(20); myServo7.period_ms(20); myServo8.period_ms(20);

}