sebastian martinez / Mbed 2 deprecated Avance_Color

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

mover.cpp@1:59be7d7e1b54, 2018-11-20 (annotated)

Committer:
sebasmartinez
Date:
Tue Nov 20 01:48:09 2018 +0000
Revision:
1:59be7d7e1b54
Parent:
0:73e805ecf765 
Programa_final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sebasmartinez 0:73e805ecf765 1 #include "mover.h"
sebasmartinez 0:73e805ecf765 2 #include "mbed.h"
sebasmartinez 0:73e805ecf765 3 #include "math.h"
sebasmartinez 0:73e805ecf765 4 /*
sebasmartinez 0:73e805ecf765 5 PwmOut Servo1(PB_14); //ROJO
sebasmartinez 0:73e805ecf765 6 PwmOut Servo2(PB_0); //BLANCO
sebasmartinez 0:73e805ecf765 7 PwmOut Servo3(PA_11); //VERDE
sebasmartinez 0:73e805ecf765 8 PwmOut Servo4(PA_8); //NEGRO
sebasmartinez 0:73e805ecf765 9 PwmOut Servo5(PB_6); //NARANJA
sebasmartinez 0:73e805ecf765 10 PwmOut Servo6(PB_1); //CAFE
sebasmartinez 0:73e805ecf765 11 PwmOut Servo7(PA_10); //AZUL
sebasmartinez 0:73e805ecf765 12 PwmOut Servo8(PA_6); //AMARILLO
sebasmartinez 0:73e805ecf765 13 */
sebasmartinez 0:73e805ecf765 14 PwmOut Servo1(PA_5); //ROJO
sebasmartinez 0:73e805ecf765 15 PwmOut Servo2(PA_6); //BLANCO
sebasmartinez 1:59be7d7e1b54 16 PwmOut Servo3(PB_2); //VERDE
sebasmartinez 0:73e805ecf765 17 PwmOut Servo4(PB_6); //NEGRO
sebasmartinez 0:73e805ecf765 18 PwmOut Servo5(PC_7); //NARANJA
sebasmartinez 0:73e805ecf765 19 PwmOut Servo6(PA_9); //CAFE
sebasmartinez 0:73e805ecf765 20 PwmOut Servo7(PA_8); //AZUL
sebasmartinez 0:73e805ecf765 21 PwmOut Servo8(PB_10); //AMARILLO
sebasmartinez 0:73e805ecf765 22
sebasmartinez 0:73e805ecf765 23 /*
sebasmartinez 0:73e805ecf765 24 PwmOut Servo6(PB_1); //CAFE
sebasmartinez 0:73e805ecf765 25 PwmOut Servo7(PA_7); //AZUL
sebasmartinez 0:73e805ecf765 26 PwmOut Servo8(PA_6); //AMARILLO
sebasmartinez 0:73e805ecf765 27
sebasmartinez 0:73e805ecf765 28 uint8_t ss_time=50; // tiempo de espera para moverse 1 mm en microsegundos
sebasmartinez 0:73e805ecf765 29
sebasmartinez 0:73e805ecf765 30 void put_sstime(uint8_t vtime){
sebasmartinez 0:73e805ecf765 31 ss_time=vtime;
sebasmartinez 0:73e805ecf765 32
sebasmartinez 0:73e805ecf765 33 }
sebasmartinez 0:73e805ecf765 34 */
sebasmartinez 0:73e805ecf765 35 int coord2us(float coord)
sebasmartinez 0:73e805ecf765 36 {
sebasmartinez 0:73e805ecf765 37 if(0 <= coord <= MAXPOS)
sebasmartinez 0:73e805ecf765 38 { return int(500+coord*2000/180);// u6
sebasmartinez 0:73e805ecf765 39 }
sebasmartinez 0:73e805ecf765 40 return 500;
sebasmartinez 0:73e805ecf765 41 /*
sebasmartinez 0:73e805ecf765 42 Secuencia 0, 180, 135, 90, 45, 0.
sebasmartinez 0:73e805ecf765 43 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
sebasmartinez 0:73e805ecf765 44 Grados: 180-B4, 135-87, 90-5A, 45-2D, 0-00;
sebasmartinez 0:73e805ecf765 45 */
sebasmartinez 0:73e805ecf765 46 }
sebasmartinez 0:73e805ecf765 47
sebasmartinez 0:73e805ecf765 48
sebasmartinez 0:73e805ecf765 49
sebasmartinez 0:73e805ecf765 50
sebasmartinez 0:73e805ecf765 51
sebasmartinez 0:73e805ecf765 52 void mover_ser(uint8_t motor, uint8_t pos){
sebasmartinez 0:73e805ecf765 53
sebasmartinez 0:73e805ecf765 54 int pulseX = coord2us(pos);
sebasmartinez 0:73e805ecf765 55 //int pulseX = 900;
sebasmartinez 0:73e805ecf765 56 //int pulseY = 1500;
sebasmartinez 0:73e805ecf765 57 // myServo1.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 58 switch ( motor ) {
sebasmartinez 0:73e805ecf765 59
sebasmartinez 0:73e805ecf765 60 case 0x01:
sebasmartinez 0:73e805ecf765 61 Servo1.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 62 break;
sebasmartinez 0:73e805ecf765 63 case 0x02:
sebasmartinez 0:73e805ecf765 64 Servo2.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 65 break;
sebasmartinez 0:73e805ecf765 66 case 0x03:
sebasmartinez 0:73e805ecf765 67 Servo3.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 68 break;
sebasmartinez 0:73e805ecf765 69 case 0x04:
sebasmartinez 0:73e805ecf765 70 Servo4.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 71 break;
sebasmartinez 0:73e805ecf765 72 case 0x05:
sebasmartinez 0:73e805ecf765 73 Servo5.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 74 break;
sebasmartinez 0:73e805ecf765 75 case 0x06:
sebasmartinez 0:73e805ecf765 76 Servo6.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 77 break;
sebasmartinez 0:73e805ecf765 78 case 0x07:
sebasmartinez 0:73e805ecf765 79 Servo7.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 80 break;
sebasmartinez 0:73e805ecf765 81 case 0x08:
sebasmartinez 0:73e805ecf765 82 Servo8.pulsewidth_us(pulseX);
sebasmartinez 0:73e805ecf765 83 break;
sebasmartinez 0:73e805ecf765 84
sebasmartinez 0:73e805ecf765 85 default: break;
sebasmartinez 0:73e805ecf765 86 }
sebasmartinez 0:73e805ecf765 87
sebasmartinez 0:73e805ecf765 88 }
sebasmartinez 0:73e805ecf765 89 void mover_par(uint8_t pares, uint8_t dir){
sebasmartinez 0:73e805ecf765 90
sebasmartinez 0:73e805ecf765 91 switch ( pares ) {
sebasmartinez 0:73e805ecf765 92
sebasmartinez 0:73e805ecf765 93 case 0x01:
sebasmartinez 1:59be7d7e1b54 94 if(dir == 0x01){
sebasmartinez 1:59be7d7e1b54 95 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 96 }else if(dir == 0x02){
sebasmartinez 1:59be7d7e1b54 97 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 98 }else if(dir == 0x03){
sebasmartinez 1:59be7d7e1b54 99 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 100 }else if(dir == 0x04){
sebasmartinez 1:59be7d7e1b54 101 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 102 } else{break;}
sebasmartinez 1:59be7d7e1b54 103 break;
sebasmartinez 1:59be7d7e1b54 104 case 0x02:
sebasmartinez 1:59be7d7e1b54 105 if(dir == 0x01){
sebasmartinez 1:59be7d7e1b54 106 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 107 }else if(dir == 0x02){
sebasmartinez 1:59be7d7e1b54 108 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 109 }else if(dir == 0x03){
sebasmartinez 1:59be7d7e1b54 110 Servo4.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 111 }else if(dir == 0x04){
sebasmartinez 1:59be7d7e1b54 112 Servo4.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 113 } else{break;}
sebasmartinez 1:59be7d7e1b54 114 break;
sebasmartinez 1:59be7d7e1b54 115 case 0x03:
sebasmartinez 1:59be7d7e1b54 116 if(dir == 0x01){
sebasmartinez 1:59be7d7e1b54 117 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 118 }else if(dir == 0x02){
sebasmartinez 1:59be7d7e1b54 119 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 120 }else if(dir == 0x03){
sebasmartinez 1:59be7d7e1b54 121 Servo6.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 122 }else if(dir == 0x04){
sebasmartinez 1:59be7d7e1b54 123 Servo6.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 124 } else{break;}
sebasmartinez 1:59be7d7e1b54 125 break;
sebasmartinez 1:59be7d7e1b54 126 case 0x04:
sebasmartinez 1:59be7d7e1b54 127 if(dir == 0x01){
sebasmartinez 1:59be7d7e1b54 128 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 129 }else if(dir == 0x02){
sebasmartinez 1:59be7d7e1b54 130 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 131 }else if(dir == 0x03){
sebasmartinez 1:59be7d7e1b54 132 Servo8.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 133 }else if(dir == 0x04){
sebasmartinez 1:59be7d7e1b54 134 Servo8.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 135 } else{break;}
sebasmartinez 1:59be7d7e1b54 136 break;
sebasmartinez 0:73e805ecf765 137
sebasmartinez 0:73e805ecf765 138 default: break;
sebasmartinez 0:73e805ecf765 139 }
sebasmartinez 0:73e805ecf765 140 }
sebasmartinez 0:73e805ecf765 141 void centro(uint8_t pos_p){
sebasmartinez 0:73e805ecf765 142 switch ( pos_p ) {
sebasmartinez 0:73e805ecf765 143
sebasmartinez 0:73e805ecf765 144 case 0x01:
sebasmartinez 0:73e805ecf765 145 Servo1.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 146 wait(1);
sebasmartinez 0:73e805ecf765 147 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 148 wait(1);
sebasmartinez 0:73e805ecf765 149 break;
sebasmartinez 0:73e805ecf765 150 case 0x02:
sebasmartinez 0:73e805ecf765 151 Servo3.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 152 wait(1);
sebasmartinez 0:73e805ecf765 153 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 154 wait(1);
sebasmartinez 0:73e805ecf765 155 break;
sebasmartinez 0:73e805ecf765 156 case 0x03:
sebasmartinez 0:73e805ecf765 157 Servo5.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 158 wait(1);
sebasmartinez 0:73e805ecf765 159 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 160 wait(1);
sebasmartinez 0:73e805ecf765 161 break;
sebasmartinez 0:73e805ecf765 162 case 0x04:
sebasmartinez 0:73e805ecf765 163 Servo7.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 164 wait(1);
sebasmartinez 0:73e805ecf765 165 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 166 wait(1);
sebasmartinez 0:73e805ecf765 167 break;
sebasmartinez 0:73e805ecf765 168 default: break;
sebasmartinez 0:73e805ecf765 169 }
sebasmartinez 0:73e805ecf765 170 }
sebasmartinez 1:59be7d7e1b54 171 void direccion(uint8_t lado){
sebasmartinez 1:59be7d7e1b54 172 switch ( lado ) {
sebasmartinez 1:59be7d7e1b54 173
sebasmartinez 1:59be7d7e1b54 174 case 0x01: //Derecha
sebasmartinez 1:59be7d7e1b54 175
sebasmartinez 1:59be7d7e1b54 176 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 177 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 178 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 179 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 180 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 181
sebasmartinez 1:59be7d7e1b54 182 Servo6.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 183 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 184 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 185 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 186 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 187
sebasmartinez 1:59be7d7e1b54 188 Servo4.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 189 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 190 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 191 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 192 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 193
sebasmartinez 1:59be7d7e1b54 194 Servo8.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 195 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 196 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 197 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 198 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 199
sebasmartinez 1:59be7d7e1b54 200 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 201 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 202 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 203 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 204 wait(1);
sebasmartinez 1:59be7d7e1b54 205 break;
sebasmartinez 1:59be7d7e1b54 206 case 0x02: //Izquierda
sebasmartinez 1:59be7d7e1b54 207 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 208 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 209 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 210 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 211 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 212
sebasmartinez 1:59be7d7e1b54 213 Servo6.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 214 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 215 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 216 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 217 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 218
sebasmartinez 1:59be7d7e1b54 219 Servo4.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 220 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 221 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 222 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 223 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 224
sebasmartinez 1:59be7d7e1b54 225 Servo8.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 226 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 227 wait_ms(300);
sebasmartinez 1:59be7d7e1b54 228 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 229 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 230
sebasmartinez 1:59be7d7e1b54 231 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 232 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 233 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 234 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 235 wait(1);
sebasmartinez 1:59be7d7e1b54 236 break;
sebasmartinez 1:59be7d7e1b54 237 case 0x03: // Adelante
sebasmartinez 1:59be7d7e1b54 238 Servo1.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 239 Servo5.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 240 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 241 Servo2.pulsewidth_us(1100);//adelante
sebasmartinez 1:59be7d7e1b54 242 Servo6.pulsewidth_us(900);//adelante
sebasmartinez 1:59be7d7e1b54 243 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 244 Servo1.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 245 Servo5.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 246 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 247 Servo2.pulsewidth_us(900);//atras
sebasmartinez 1:59be7d7e1b54 248 Servo6.pulsewidth_us(1200);//atras
sebasmartinez 1:59be7d7e1b54 249 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 250 Servo3.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 251 Servo7.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 252 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 253 Servo4.pulsewidth_us(2000);//adelante
sebasmartinez 1:59be7d7e1b54 254 Servo8.pulsewidth_us(1900);//adelante
sebasmartinez 1:59be7d7e1b54 255 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 256 Servo3.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 257 Servo7.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 258 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 259 Servo4.pulsewidth_us(1700);//atras
sebasmartinez 1:59be7d7e1b54 260 Servo8.pulsewidth_us(2100);//atras
sebasmartinez 1:59be7d7e1b54 261 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 262
sebasmartinez 1:59be7d7e1b54 263
sebasmartinez 1:59be7d7e1b54 264 break;
sebasmartinez 1:59be7d7e1b54 265 case 0x04: //Atras
sebasmartinez 1:59be7d7e1b54 266
sebasmartinez 1:59be7d7e1b54 267 Servo1.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 268 Servo5.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 269 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 270 Servo2.pulsewidth_us(1100);//adelante
sebasmartinez 1:59be7d7e1b54 271 Servo6.pulsewidth_us(900);//adelante
sebasmartinez 1:59be7d7e1b54 272 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 273 Servo1.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 274 Servo5.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 275 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 276 Servo2.pulsewidth_us(900);//atras
sebasmartinez 1:59be7d7e1b54 277 Servo6.pulsewidth_us(1200);//atras
sebasmartinez 1:59be7d7e1b54 278 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 279
sebasmartinez 1:59be7d7e1b54 280 Servo3.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 281 Servo7.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 282 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 283 Servo4.pulsewidth_us(2000);//adelante
sebasmartinez 1:59be7d7e1b54 284 Servo8.pulsewidth_us(1900);//adelante
sebasmartinez 1:59be7d7e1b54 285 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 286 Servo3.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 287 Servo7.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 288 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 289 Servo4.pulsewidth_us(1700);//atras
sebasmartinez 1:59be7d7e1b54 290 Servo8.pulsewidth_us(2100);//atras
sebasmartinez 1:59be7d7e1b54 291 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 292
sebasmartinez 1:59be7d7e1b54 293
sebasmartinez 1:59be7d7e1b54 294
sebasmartinez 1:59be7d7e1b54 295
sebasmartinez 1:59be7d7e1b54 296 break;
sebasmartinez 1:59be7d7e1b54 297 case 0x05: //Saludar1
sebasmartinez 1:59be7d7e1b54 298 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 299 Servo8.pulsewidth_us(FRONT);
sebasmartinez 1:59be7d7e1b54 300 Servo4.pulsewidth_us(BACK);
sebasmartinez 1:59be7d7e1b54 301 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 302
sebasmartinez 1:59be7d7e1b54 303 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 304 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 305 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 306 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 307 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 308 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 309 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 310 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 311 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 312 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 313 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 314 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 315 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 316 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 317 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 318 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 319 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 320 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 321 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 322 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 323 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 324 wait_ms(2000);
sebasmartinez 1:59be7d7e1b54 325
sebasmartinez 1:59be7d7e1b54 326
sebasmartinez 1:59be7d7e1b54 327 break;
sebasmartinez 1:59be7d7e1b54 328 case 0x06: //Saludar2
sebasmartinez 1:59be7d7e1b54 329 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 330 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 331 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 332 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 333
sebasmartinez 1:59be7d7e1b54 334 Servo2.pulsewidth_us(FRONT);
sebasmartinez 1:59be7d7e1b54 335 Servo4.pulsewidth_us(FRONT);
sebasmartinez 1:59be7d7e1b54 336 Servo8.pulsewidth_us(FRONT);
sebasmartinez 1:59be7d7e1b54 337 Servo6.pulsewidth_us(FRONT);
sebasmartinez 1:59be7d7e1b54 338 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 339 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 340 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 341 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 342 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 343 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 344
sebasmartinez 1:59be7d7e1b54 345 Servo8.pulsewidth_us(BACK);
sebasmartinez 1:59be7d7e1b54 346 Servo6.pulsewidth_us(BACK);
sebasmartinez 1:59be7d7e1b54 347 Servo2.pulsewidth_us(BACK);
sebasmartinez 1:59be7d7e1b54 348 Servo4.pulsewidth_us(BACK);
sebasmartinez 1:59be7d7e1b54 349 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 350 break;
sebasmartinez 1:59be7d7e1b54 351
sebasmartinez 1:59be7d7e1b54 352 case 0x07: //Pruebas
sebasmartinez 1:59be7d7e1b54 353
sebasmartinez 1:59be7d7e1b54 354 Servo1.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 355 Servo5.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 356 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 357 Servo2.pulsewidth_us(1100);//adelante
sebasmartinez 1:59be7d7e1b54 358 Servo6.pulsewidth_us(900);//adelante
sebasmartinez 1:59be7d7e1b54 359 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 360 Servo1.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 361 Servo5.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 362 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 363 Servo2.pulsewidth_us(900);//atras
sebasmartinez 1:59be7d7e1b54 364 Servo6.pulsewidth_us(1200);//atras
sebasmartinez 1:59be7d7e1b54 365 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 366
sebasmartinez 1:59be7d7e1b54 367 Servo3.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 368 Servo7.pulsewidth_us(2000);//abajo
sebasmartinez 1:59be7d7e1b54 369 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 370 Servo4.pulsewidth_us(2000);//adelante
sebasmartinez 1:59be7d7e1b54 371 Servo8.pulsewidth_us(1900);//adelante
sebasmartinez 1:59be7d7e1b54 372 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 373 Servo3.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 374 Servo7.pulsewidth_us(1500);//arriba
sebasmartinez 1:59be7d7e1b54 375 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 376 Servo4.pulsewidth_us(1700);//atras
sebasmartinez 1:59be7d7e1b54 377 Servo8.pulsewidth_us(2100);//atras
sebasmartinez 1:59be7d7e1b54 378 wait_ms(80);
sebasmartinez 1:59be7d7e1b54 379
sebasmartinez 1:59be7d7e1b54 380
sebasmartinez 1:59be7d7e1b54 381
sebasmartinez 1:59be7d7e1b54 382 break;
sebasmartinez 1:59be7d7e1b54 383 default: break;
sebasmartinez 1:59be7d7e1b54 384 }
sebasmartinez 1:59be7d7e1b54 385 }
sebasmartinez 1:59be7d7e1b54 386 void arrancar()
sebasmartinez 1:59be7d7e1b54 387 {
sebasmartinez 1:59be7d7e1b54 388 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 389 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 390 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 391 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 392 wait_ms(1000);
sebasmartinez 1:59be7d7e1b54 393 Servo1.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 394 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 395 Servo1.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 396 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 397 Servo1.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 398 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 399 Servo2.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 400 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 401 Servo2.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 402 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 403 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 404 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 405
sebasmartinez 1:59be7d7e1b54 406 Servo3.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 407 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 408 Servo3.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 409 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 410 Servo3.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 411 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 412 Servo4.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 413 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 414 Servo4.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 415 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 416 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 417 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 418
sebasmartinez 1:59be7d7e1b54 419 Servo5.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 420 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 421 Servo5.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 422 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 423 Servo5.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 424 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 425 Servo6.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 426 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 427 Servo6.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 428 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 429 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 430 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 431
sebasmartinez 1:59be7d7e1b54 432 Servo7.pulsewidth_us(ARRIBA);
sebasmartinez 1:59be7d7e1b54 433 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 434 Servo7.pulsewidth_us(ABAJO);
sebasmartinez 1:59be7d7e1b54 435 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 436 Servo7.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 437 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 438 Servo8.pulsewidth_us(ADELANTE);
sebasmartinez 1:59be7d7e1b54 439 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 440 Servo8.pulsewidth_us(ATRAS);
sebasmartinez 1:59be7d7e1b54 441 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 442 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 1:59be7d7e1b54 443 wait_ms(500);
sebasmartinez 1:59be7d7e1b54 444 }
sebasmartinez 0:73e805ecf765 445 void init_servo()
sebasmartinez 0:73e805ecf765 446 {
sebasmartinez 0:73e805ecf765 447 Servo1.period_ms(20);
sebasmartinez 0:73e805ecf765 448 Servo2.period_ms(20);
sebasmartinez 0:73e805ecf765 449 Servo3.period_ms(20);
sebasmartinez 0:73e805ecf765 450 Servo4.period_ms(20);
sebasmartinez 0:73e805ecf765 451 Servo5.period_ms(20);
sebasmartinez 0:73e805ecf765 452 Servo6.period_ms(20);
sebasmartinez 0:73e805ecf765 453 Servo7.period_ms(20);
sebasmartinez 0:73e805ecf765 454 Servo8.period_ms(20);
sebasmartinez 0:73e805ecf765 455 /*
sebasmartinez 0:73e805ecf765 456 //Posicion inicial a cada servo CENTRO
sebasmartinez 0:73e805ecf765 457 Servo1.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 458 Servo2.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 459 Servo3.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 460 Servo4.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 461 Servo5.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 462 Servo6.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 463 Servo7.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 464 Servo8.pulsewidth_us(CENTRO);
sebasmartinez 0:73e805ecf765 465 */
sebasmartinez 0:73e805ecf765 466 }
sebasmartinez 0:73e805ecf765 467