sebastian martinez
/
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.
Diff: mover.cpp
- Revision:
- 1:59be7d7e1b54
- Parent:
- 0:73e805ecf765
--- a/mover.cpp Wed Oct 10 22:49:18 2018 +0000
+++ b/mover.cpp Tue Nov 20 01:48:09 2018 +0000
@@ -3,22 +3,17 @@
#include "math.h"
/*
PwmOut Servo1(PB_14); //ROJO
-
PwmOut Servo2(PB_0); //BLANCO
-
PwmOut Servo3(PA_11); //VERDE
PwmOut Servo4(PA_8); //NEGRO
PwmOut Servo5(PB_6); //NARANJA
-
PwmOut Servo6(PB_1); //CAFE
-
PwmOut Servo7(PA_10); //AZUL
-
PwmOut Servo8(PA_6); //AMARILLO
*/
PwmOut Servo1(PA_5); //ROJO
PwmOut Servo2(PA_6); //BLANCO
-PwmOut Servo3(PA_7); //VERDE
+PwmOut Servo3(PB_2); //VERDE
PwmOut Servo4(PB_6); //NEGRO
PwmOut Servo5(PC_7); //NARANJA
PwmOut Servo6(PA_9); //CAFE
@@ -96,89 +91,49 @@
switch ( pares ) {
case 0x01:
- if(dir==0x01){
- Servo1.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo2.pulsewidth_us(ADELANTE);
- wait(0.5);
- Servo1.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo2.pulsewidth_us(ATRAS);
- wait(0.5);
- }else{
- Servo1.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo2.pulsewidth_us(ATRAS);
- wait(0.5);
- Servo1.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo2.pulsewidth_us(ADELANTE);
- wait(0.5);
- }
- break;
- case 0x02:
- if(dir==0x01){
- Servo3.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo4.pulsewidth_us(ADELANTE);
- wait(0.5);
- Servo3.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo4.pulsewidth_us(ATRAS);
- wait(0.5);
- }else{
- Servo3.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo4.pulsewidth_us(ATRAS);
- wait(0.5);
- Servo3.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo4.pulsewidth_us(ADELANTE);
- wait(0.5);
- }
- break;
- case 0x03:
- if(dir==0x01){
- Servo5.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo6.pulsewidth_us(ADELANTE);
- wait(0.5);
- Servo5.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo6.pulsewidth_us(ATRAS);
- wait(0.5);
- }else{
- Servo5.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo6.pulsewidth_us(ATRAS);
- wait(0.5);
- Servo5.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo6.pulsewidth_us(ADELANTE);
- wait(0.5);
- }
- break;
- case 0x04:
- if(dir==0x01){
- Servo7.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo8.pulsewidth_us(ADELANTE);
- wait(0.5);
- Servo7.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo8.pulsewidth_us(ATRAS);
- wait(0.5);
- }else{
- Servo7.pulsewidth_us(ARRIBA);
- wait(0.5);
- Servo8.pulsewidth_us(ATRAS);
- wait(0.5);
- Servo7.pulsewidth_us(ABAJO);
- wait(0.5);
- Servo8.pulsewidth_us(ADELANTE);
- wait(0.5);
- }
- break;
+ if(dir == 0x01){
+ Servo1.pulsewidth_us(ARRIBA);
+ }else if(dir == 0x02){
+ Servo1.pulsewidth_us(ABAJO);
+ }else if(dir == 0x03){
+ Servo2.pulsewidth_us(ADELANTE);
+ }else if(dir == 0x04){
+ Servo2.pulsewidth_us(ATRAS);
+ } else{break;}
+ break;
+ case 0x02:
+ if(dir == 0x01){
+ Servo3.pulsewidth_us(ARRIBA);
+ }else if(dir == 0x02){
+ Servo3.pulsewidth_us(ABAJO);
+ }else if(dir == 0x03){
+ Servo4.pulsewidth_us(ADELANTE);
+ }else if(dir == 0x04){
+ Servo4.pulsewidth_us(ATRAS);
+ } else{break;}
+ break;
+ case 0x03:
+ if(dir == 0x01){
+ Servo5.pulsewidth_us(ARRIBA);
+ }else if(dir == 0x02){
+ Servo5.pulsewidth_us(ABAJO);
+ }else if(dir == 0x03){
+ Servo6.pulsewidth_us(ADELANTE);
+ }else if(dir == 0x04){
+ Servo6.pulsewidth_us(ATRAS);
+ } else{break;}
+ break;
+ case 0x04:
+ if(dir == 0x01){
+ Servo7.pulsewidth_us(ARRIBA);
+ }else if(dir == 0x02){
+ Servo7.pulsewidth_us(ABAJO);
+ }else if(dir == 0x03){
+ Servo8.pulsewidth_us(ADELANTE);
+ }else if(dir == 0x04){
+ Servo8.pulsewidth_us(ATRAS);
+ } else{break;}
+ break;
default: break;
}
@@ -213,6 +168,280 @@
default: break;
}
}
+void direccion(uint8_t lado){
+ switch ( lado ) {
+
+ case 0x01: //Derecha
+
+ Servo2.pulsewidth_us(ADELANTE);
+ Servo1.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo1.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo6.pulsewidth_us(ADELANTE);
+ Servo5.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo5.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo4.pulsewidth_us(ADELANTE);
+ Servo3.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo3.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo8.pulsewidth_us(ADELANTE);
+ Servo7.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo7.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo2.pulsewidth_us(CENTRO);
+ Servo4.pulsewidth_us(CENTRO);
+ Servo6.pulsewidth_us(CENTRO);
+ Servo8.pulsewidth_us(CENTRO);
+ wait(1);
+ break;
+ case 0x02: //Izquierda
+ Servo2.pulsewidth_us(ATRAS);
+ Servo1.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo1.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo6.pulsewidth_us(ATRAS);
+ Servo5.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo5.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo4.pulsewidth_us(ATRAS);
+ Servo3.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo3.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo8.pulsewidth_us(ATRAS);
+ Servo7.pulsewidth_us(ARRIBA);
+ wait_ms(300);
+ Servo7.pulsewidth_us(ABAJO);
+ wait_ms(500);
+
+ Servo2.pulsewidth_us(CENTRO);
+ Servo4.pulsewidth_us(CENTRO);
+ Servo6.pulsewidth_us(CENTRO);
+ Servo8.pulsewidth_us(CENTRO);
+ wait(1);
+ break;
+ case 0x03: // Adelante
+ Servo1.pulsewidth_us(1500);//arriba
+ Servo5.pulsewidth_us(1500);//arriba
+ wait_ms(80);
+ Servo2.pulsewidth_us(1100);//adelante
+ Servo6.pulsewidth_us(900);//adelante
+ wait_ms(80);
+ Servo1.pulsewidth_us(2000);//abajo
+ Servo5.pulsewidth_us(2000);//abajo
+ wait_ms(80);
+ Servo2.pulsewidth_us(900);//atras
+ Servo6.pulsewidth_us(1200);//atras
+ wait_ms(80);
+ Servo3.pulsewidth_us(1500);//arriba
+ Servo7.pulsewidth_us(1500);//arriba
+ wait_ms(80);
+ Servo4.pulsewidth_us(2000);//adelante
+ Servo8.pulsewidth_us(1900);//adelante
+ wait_ms(80);
+ Servo3.pulsewidth_us(2000);//abajo
+ Servo7.pulsewidth_us(2000);//abajo
+ wait_ms(80);
+ Servo4.pulsewidth_us(1700);//atras
+ Servo8.pulsewidth_us(2100);//atras
+ wait_ms(80);
+
+
+ break;
+ case 0x04: //Atras
+
+ Servo1.pulsewidth_us(2000);//abajo
+ Servo5.pulsewidth_us(2000);//abajo
+ wait_ms(80);
+ Servo2.pulsewidth_us(1100);//adelante
+ Servo6.pulsewidth_us(900);//adelante
+ wait_ms(80);
+ Servo1.pulsewidth_us(1500);//arriba
+ Servo5.pulsewidth_us(1500);//arriba
+ wait_ms(80);
+ Servo2.pulsewidth_us(900);//atras
+ Servo6.pulsewidth_us(1200);//atras
+ wait_ms(80);
+
+ Servo3.pulsewidth_us(2000);//abajo
+ Servo7.pulsewidth_us(2000);//abajo
+ wait_ms(80);
+ Servo4.pulsewidth_us(2000);//adelante
+ Servo8.pulsewidth_us(1900);//adelante
+ wait_ms(80);
+ Servo3.pulsewidth_us(1500);//arriba
+ Servo7.pulsewidth_us(1500);//arriba
+ wait_ms(80);
+ Servo4.pulsewidth_us(1700);//atras
+ Servo8.pulsewidth_us(2100);//atras
+ wait_ms(80);
+
+
+
+
+ break;
+ case 0x05: //Saludar1
+ Servo2.pulsewidth_us(CENTRO);
+ Servo8.pulsewidth_us(FRONT);
+ Servo4.pulsewidth_us(BACK);
+ Servo6.pulsewidth_us(CENTRO);
+
+ Servo3.pulsewidth_us(ABAJO);
+ Servo5.pulsewidth_us(ABAJO);
+ Servo7.pulsewidth_us(ABAJO);
+ wait_ms(500);
+ Servo1.pulsewidth_us(ARRIBA);
+ Servo2.pulsewidth_us(ADELANTE);
+ wait_ms(500);
+ Servo2.pulsewidth_us(ATRAS);
+ wait_ms(500);
+ Servo2.pulsewidth_us(ADELANTE);
+ wait_ms(500);
+ Servo2.pulsewidth_us(ATRAS);
+ wait_ms(500);
+ Servo2.pulsewidth_us(ADELANTE);
+ wait_ms(500);
+ Servo2.pulsewidth_us(ATRAS);
+ wait_ms(500);
+ Servo2.pulsewidth_us(CENTRO);
+ Servo1.pulsewidth_us(ABAJO);
+ Servo4.pulsewidth_us(CENTRO);
+ Servo8.pulsewidth_us(CENTRO);
+ wait_ms(2000);
+
+
+ break;
+ case 0x06: //Saludar2
+ Servo1.pulsewidth_us(ARRIBA);
+ Servo3.pulsewidth_us(ARRIBA);
+ Servo7.pulsewidth_us(ABAJO);
+ Servo5.pulsewidth_us(ABAJO);
+
+ Servo2.pulsewidth_us(FRONT);
+ Servo4.pulsewidth_us(FRONT);
+ Servo8.pulsewidth_us(FRONT);
+ Servo6.pulsewidth_us(FRONT);
+ wait_ms(500);
+ Servo1.pulsewidth_us(ABAJO);
+ Servo3.pulsewidth_us(ABAJO);
+ Servo3.pulsewidth_us(ABAJO);
+ Servo7.pulsewidth_us(ARRIBA);
+ Servo5.pulsewidth_us(ARRIBA);
+
+ Servo8.pulsewidth_us(BACK);
+ Servo6.pulsewidth_us(BACK);
+ Servo2.pulsewidth_us(BACK);
+ Servo4.pulsewidth_us(BACK);
+ wait_ms(500);
+ break;
+
+ case 0x07: //Pruebas
+
+ Servo1.pulsewidth_us(2000);//abajo
+ Servo5.pulsewidth_us(2000);//abajo
+ wait_ms(80);
+ Servo2.pulsewidth_us(1100);//adelante
+ Servo6.pulsewidth_us(900);//adelante
+ wait_ms(80);
+ Servo1.pulsewidth_us(1500);//arriba
+ Servo5.pulsewidth_us(1500);//arriba
+ wait_ms(80);
+ Servo2.pulsewidth_us(900);//atras
+ Servo6.pulsewidth_us(1200);//atras
+ wait_ms(80);
+
+ Servo3.pulsewidth_us(2000);//abajo
+ Servo7.pulsewidth_us(2000);//abajo
+ wait_ms(80);
+ Servo4.pulsewidth_us(2000);//adelante
+ Servo8.pulsewidth_us(1900);//adelante
+ wait_ms(80);
+ Servo3.pulsewidth_us(1500);//arriba
+ Servo7.pulsewidth_us(1500);//arriba
+ wait_ms(80);
+ Servo4.pulsewidth_us(1700);//atras
+ Servo8.pulsewidth_us(2100);//atras
+ wait_ms(80);
+
+
+
+ break;
+ default: break;
+}
+ }
+void arrancar()
+{
+ Servo1.pulsewidth_us(ABAJO);
+ Servo3.pulsewidth_us(ABAJO);
+ Servo5.pulsewidth_us(ABAJO);
+ Servo7.pulsewidth_us(ABAJO);
+wait_ms(1000);
+ Servo1.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo1.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo1.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo2.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo2.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo2.pulsewidth_us(CENTRO);
+wait_ms(500);
+
+Servo3.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo3.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo3.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo4.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo4.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo4.pulsewidth_us(CENTRO);
+wait_ms(500);
+
+Servo5.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo5.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo5.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo6.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo6.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo6.pulsewidth_us(CENTRO);
+wait_ms(500);
+
+Servo7.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo7.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo7.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo8.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo8.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo8.pulsewidth_us(CENTRO);
+wait_ms(500);
+ }
void init_servo()
{
Servo1.period_ms(20);