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:
- 0:73e805ecf765
- Child:
- 1:59be7d7e1b54
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mover.cpp Wed Oct 10 22:49:18 2018 +0000
@@ -0,0 +1,238 @@
+#include "mover.h"
+#include "mbed.h"
+#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 Servo4(PB_6); //NEGRO
+PwmOut Servo5(PC_7); //NARANJA
+PwmOut Servo6(PA_9); //CAFE
+PwmOut Servo7(PA_8); //AZUL
+PwmOut Servo8(PB_10); //AMARILLO
+
+/*
+PwmOut Servo6(PB_1); //CAFE
+PwmOut Servo7(PA_7); //AZUL
+PwmOut Servo8(PA_6); //AMARILLO
+
+uint8_t ss_time=50; // tiempo de espera para moverse 1 mm en microsegundos
+
+void put_sstime(uint8_t vtime){
+ ss_time=vtime;
+
+}
+*/
+int coord2us(float coord)
+{
+ if(0 <= coord <= MAXPOS)
+ { return int(500+coord*2000/180);// u6
+ }
+ return 500;
+/*
+Secuencia 0, 180, 135, 90, 45, 0.
+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
+Grados: 180-B4, 135-87, 90-5A, 45-2D, 0-00;
+*/
+}
+
+
+
+
+
+void mover_ser(uint8_t motor, uint8_t pos){
+
+ int pulseX = coord2us(pos);
+ //int pulseX = 900;
+ //int pulseY = 1500;
+ // myServo1.pulsewidth_us(pulseX);
+ switch ( motor ) {
+
+ case 0x01:
+ Servo1.pulsewidth_us(pulseX);
+ break;
+ case 0x02:
+ Servo2.pulsewidth_us(pulseX);
+ break;
+ case 0x03:
+ Servo3.pulsewidth_us(pulseX);
+ break;
+ case 0x04:
+ Servo4.pulsewidth_us(pulseX);
+ break;
+ case 0x05:
+ Servo5.pulsewidth_us(pulseX);
+ break;
+ case 0x06:
+ Servo6.pulsewidth_us(pulseX);
+ break;
+ case 0x07:
+ Servo7.pulsewidth_us(pulseX);
+ break;
+ case 0x08:
+ Servo8.pulsewidth_us(pulseX);
+ break;
+
+ default: break;
+}
+
+}
+void mover_par(uint8_t pares, uint8_t dir){
+
+ 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;
+
+ default: break;
+}
+ }
+void centro(uint8_t pos_p){
+switch ( pos_p ) {
+
+ case 0x01:
+ Servo1.pulsewidth_us(CENTRO);
+ wait(1);
+ Servo2.pulsewidth_us(CENTRO);
+ wait(1);
+ break;
+ case 0x02:
+ Servo3.pulsewidth_us(CENTRO);
+ wait(1);
+ Servo4.pulsewidth_us(CENTRO);
+ wait(1);
+ break;
+ case 0x03:
+ Servo5.pulsewidth_us(CENTRO);
+ wait(1);
+ Servo6.pulsewidth_us(CENTRO);
+ wait(1);
+ break;
+ case 0x04:
+ Servo7.pulsewidth_us(CENTRO);
+ wait(1);
+ Servo8.pulsewidth_us(CENTRO);
+ wait(1);
+ break;
+ default: break;
+}
+ }
+void init_servo()
+{
+ Servo1.period_ms(20);
+ Servo2.period_ms(20);
+ Servo3.period_ms(20);
+ Servo4.period_ms(20);
+ Servo5.period_ms(20);
+ Servo6.period_ms(20);
+ Servo7.period_ms(20);
+ Servo8.period_ms(20);
+ /*
+ //Posicion inicial a cada servo CENTRO
+ Servo1.pulsewidth_us(CENTRO);
+ Servo2.pulsewidth_us(CENTRO);
+ Servo3.pulsewidth_us(CENTRO);
+ Servo4.pulsewidth_us(CENTRO);
+ Servo5.pulsewidth_us(CENTRO);
+ Servo6.pulsewidth_us(CENTRO);
+ Servo7.pulsewidth_us(CENTRO);
+ Servo8.pulsewidth_us(CENTRO);
+ */
+}
+