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 }