Primer_corte_avance
Fork of 01-04EntregaPrimerCorte by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Sat Jul 16 2022 08:45:13 by
1.7.2
