movimiento de 8 servos comandos hexadecimal
Fork of 01-04EntregaPrimerCorte by
mover.cpp
00001 00002 #include "mover.h" 00003 #include "mbed.h" 00004 #include "math.h" 00005 00006 00007 PwmOut myServo1(PB_3);// definir puertos de los servos 00008 PwmOut myServo2(PB_4); 00009 PwmOut myServo3(PB_5); 00010 PwmOut myServo4(PB_8); 00011 PwmOut myServo5(PB_9); 00012 PwmOut myServo6(PA_5); 00013 PwmOut myServo7(PA_6); 00014 PwmOut myServo8(PC_8); 00015 00016 00017 uint8_t ss_time=50; // tiempo de espera para moverse 1 mm en microsegundos 00018 00019 void put_sstime(uint8_t vtime){ 00020 ss_time=vtime; // cargar tiempo 00021 00022 } 00023 00024 int coord2us(float coord) 00025 { 00026 if(0 <= coord <= MAXPOS) 00027 return int(750+coord*1900/50);// cambiar de numero de entrada a grados 00028 return 750; 00029 00030 } 00031 00032 void mover_ser(uint8_t nmotor, uint8_t grados){// definimos que leen los 8 servos y le asiganamos a cada variable un servo 00033 00034 int pulsea = coord2us(grados); 00035 int pulseb = coord2us(grados); 00036 int pulsec = coord2us(grados); 00037 int pulsed = coord2us(grados); 00038 int pulsee = coord2us(grados); 00039 int pulsef = coord2us(grados); 00040 int pulseg = coord2us(grados); 00041 int pulseh = coord2us(grados); 00042 switch (nmotor) { 00043 case 0x01: myServo1.pulsewidth_us(pulsea);break; // caso para cada servo 00044 case 0x02: myServo2.pulsewidth_us(pulseb);break; 00045 case 0x03: myServo2.pulsewidth_us(pulsec);break; 00046 case 0x04: myServo2.pulsewidth_us(pulsed);break; 00047 case 0x05: myServo2.pulsewidth_us(pulsee);break; 00048 case 0x06: myServo2.pulsewidth_us(pulsef);break; 00049 case 0x07: myServo2.pulsewidth_us(pulseg);break; 00050 case 0x08: myServo2.pulsewidth_us(pulseh);break; 00051 //default: debug_m("numero de motor no valido");break ; 00052 00053 00054 } 00055 } 00056 00057 void init_servo() 00058 { 00059 myServo1.period_ms(20);// periodo de cada servo 00060 myServo2.period_ms(20); 00061 myServo3.period_ms(20); 00062 myServo4.period_ms(20); 00063 myServo5.period_ms(20); 00064 myServo6.period_ms(20); 00065 myServo7.period_ms(20); 00066 myServo8.period_ms(20); 00067 00068 }
Generated on Wed Jul 20 2022 02:05:06 by
1.7.2
