Primer_corte_avance

Dependencies:   mbed

Fork of 01-04EntregaPrimerCorte by ferney alberto beltran molina

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers mover.cpp Source File

mover.cpp

00001 
00002 #include "mover.h"
00003 #include "mbed.h"
00004 #include "math.h"
00005 
00006 
00007 PwmOut Servo1(PA_1);
00008 PwmOut Servo2(PB_8);
00009 PwmOut Servo3(PB_14);
00010 PwmOut Servo4(PB_15);
00011 PwmOut Servo5(PA_8);
00012 PwmOut Servo6(PB_6);
00013 PwmOut Servo7(PA_9);
00014 PwmOut Servo8(PA_10);
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;
00021     
00022 }
00023 
00024 int coord2us(float coord)
00025 {
00026     if(0 <= coord <= MAXPOS)
00027         return int(500+coord*2000/180);// u6
00028     return 500;
00029 /*
00030 Secuencia 0, 180, 135, 90, 45, 0.
00031 FF 01 01 00 FE FF 01 01 B4 FE FF 01 01 87 FE FF 01 01 5A FE FF 01 01 2D FE FF 01 01 00 FE
00032 Grados: 180-B4, 135-87, 90-5A, 45-2D, 0-00;
00033 */
00034 }
00035 
00036 
00037  
00038 
00039 
00040 void mover_ser(uint8_t motor, uint8_t pos){
00041 
00042     int pulseX = coord2us(pos);
00043     //int pulseX = 900;
00044     //int pulseY = 1500;
00045    // myServo1.pulsewidth_us(pulseX);
00046  switch ( motor ) {
00047   
00048   case 0x01: 
00049      Servo1.pulsewidth_us(pulseX);
00050     break;
00051   case 0x02: 
00052      Servo2.pulsewidth_us(pulseX);
00053     break;    
00054   case 0x03: 
00055      Servo3.pulsewidth_us(pulseX);
00056     break;
00057   case 0x04: 
00058      Servo4.pulsewidth_us(pulseX);
00059     break;
00060   case 0x05: 
00061      Servo5.pulsewidth_us(pulseX);
00062     break;
00063   case 0x06: 
00064      Servo6.pulsewidth_us(pulseX);
00065     break;
00066   case 0x07: 
00067      Servo7.pulsewidth_us(pulseX);
00068     break;   
00069   case 0x08: 
00070      Servo8.pulsewidth_us(pulseX);
00071     break;
00072             
00073   default: break;
00074 }
00075 
00076 }
00077 void mover_par(uint8_t pares, uint8_t dir){
00078      
00079      switch ( pares ) {
00080   
00081   case 0x01: 
00082         if(dir==0x01){
00083             Servo1.pulsewidth_us(ARRIBA);
00084             wait(0.5);
00085             Servo2.pulsewidth_us(ADELANTE);
00086             wait(0.5);
00087             Servo1.pulsewidth_us(ABAJO);
00088             wait(0.5);
00089             Servo2.pulsewidth_us(ATRAS);
00090             wait(0.5);
00091          }else{
00092             Servo1.pulsewidth_us(ARRIBA);
00093             wait(0.5);
00094             Servo2.pulsewidth_us(ATRAS);
00095             wait(0.5);
00096             Servo1.pulsewidth_us(ABAJO);
00097             wait(0.5);
00098             Servo2.pulsewidth_us(ADELANTE);
00099             wait(0.5);
00100           }
00101     break;
00102   case 0x02: 
00103      if(dir==0x01){
00104             Servo1.pulsewidth_us(ARRIBA);
00105             wait(0.5);
00106             Servo2.pulsewidth_us(ADELANTE);
00107             wait(0.5);
00108             Servo1.pulsewidth_us(ABAJO);
00109             wait(0.5);
00110             Servo2.pulsewidth_us(ATRAS);
00111             wait(0.5);
00112          }else{
00113             Servo1.pulsewidth_us(ARRIBA);
00114             wait(0.5);
00115             Servo2.pulsewidth_us(ATRAS);
00116             wait(0.5);
00117             Servo1.pulsewidth_us(ABAJO);
00118             wait(0.5);
00119             Servo2.pulsewidth_us(ADELANTE);
00120             wait(0.5);
00121           }
00122     break;    
00123   case 0x03: 
00124      if(dir==0x01){
00125             Servo1.pulsewidth_us(ARRIBA);
00126             wait(0.5);
00127             Servo2.pulsewidth_us(ADELANTE);
00128             wait(0.5);
00129             Servo1.pulsewidth_us(ABAJO);
00130             wait(0.5);
00131             Servo2.pulsewidth_us(ATRAS);
00132             wait(0.5);
00133          }else{
00134             Servo1.pulsewidth_us(ARRIBA);
00135             wait(0.5);
00136             Servo2.pulsewidth_us(ATRAS);
00137             wait(0.5);
00138             Servo1.pulsewidth_us(ABAJO);
00139             wait(0.5);
00140             Servo2.pulsewidth_us(ADELANTE);
00141             wait(0.5);
00142           }
00143     break;
00144   case 0x04: 
00145      if(dir==0x01){
00146             Servo1.pulsewidth_us(ARRIBA);
00147             wait(0.5);
00148             Servo2.pulsewidth_us(ADELANTE);
00149             wait(0.5);
00150             Servo1.pulsewidth_us(ABAJO);
00151             wait(0.5);
00152             Servo2.pulsewidth_us(ATRAS);
00153             wait(0.5);
00154          }else{
00155             Servo1.pulsewidth_us(ARRIBA);
00156             wait(0.5);
00157             Servo2.pulsewidth_us(ATRAS);
00158             wait(0.5);
00159             Servo1.pulsewidth_us(ABAJO);
00160             wait(0.5);
00161             Servo2.pulsewidth_us(ADELANTE);
00162             wait(0.5);
00163           }
00164     break;
00165      default: break;
00166 }
00167     }
00168 void centro(uint8_t pos_p){
00169 switch ( pos_p ) {
00170   
00171   case 0x01: 
00172             Servo1.pulsewidth_us(CENTRO);
00173             wait(1);
00174             Servo2.pulsewidth_us(CENTRO);
00175             wait(1);
00176     break;
00177   case 0x02: 
00178             Servo3.pulsewidth_us(CENTRO);
00179             wait(1);
00180             Servo4.pulsewidth_us(CENTRO);
00181             wait(1);
00182     break;    
00183   case 0x03: 
00184             Servo5.pulsewidth_us(CENTRO);
00185             wait(1);
00186             Servo6.pulsewidth_us(CENTRO);
00187             wait(1);
00188     break;
00189   case 0x04: 
00190             Servo7.pulsewidth_us(CENTRO);
00191             wait(1);
00192             Servo8.pulsewidth_us(CENTRO);
00193             wait(1);
00194     break;
00195       default: break;
00196 }
00197     }    
00198 void init_servo()
00199 {
00200    Servo1.period_ms(20);
00201    Servo2.period_ms(20);
00202    Servo3.period_ms(20);
00203    Servo4.period_ms(20);
00204    Servo5.period_ms(20);
00205    Servo6.period_ms(20);
00206    Servo7.period_ms(20);
00207    Servo8.period_ms(20);
00208     
00209 }
00210