Revision 0:22b33429aff5, committed 2018-11-22

Comitter:

sammir

Date:

Thu Nov 22 01:19:55 2018 +0000

Commit message:

TRABAJO FINAL CUADRUPEDO

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 22 01:19:55 2018 +0000
@@ -0,0 +1,152 @@
+r#include "mbed.h"
+#include "main.h"
+Timer tiempo;
+Serial command(USBTX, USBRX);
+
+DigitalIn entrada (PC_9);
+DigitalOut S0 (PA_9);
+DigitalOut S1 (PA_8);
+DigitalOut S3 (PC_7);
+DigitalOut S2 (PA_10);
+
+int valor;
+int color=0;
+int rojo=0;
+int azul=0;
+int verde=0;
+int Detectar();
+int tme=0;
+
+int main() {
+    
+    init_serial();    
+    
+    
+    debug_m("inicio \n");
+    uint32_t read_cc;   
+    while(1)
+    {
+        read_cc=read_command();        
+        switch (read_cc) {
+            case  0x01: moving(); break ;             
+            case  0x02: moving2();break ;              
+            case  0x03: sensor();break ;  
+            case  0x04:moving3();break;
+        }
+    }
+}
+
+uint32_t read_command()
+{
+   // retorna los byte recibidos concatenados en un entero,
+      
+    char intc=command.getc();    
+    while(intc != 0xFF)
+    intc=command.getc();
+    return command.getc();
+}
+void init_serial()
+{
+    command.baud(9600);    
+}
+void moving(){
+    debug_m("se inicia el comado mover..\n");   
+    char nmotor=command.getc();
+    char pos=command.getc();
+    char endc=command.getc();
+    mover_ser(nmotor,pos); 
+    debug_m("fin del comado guardar..\n");    
+} 
+void moving2(){
+    debug_m("se inicia el comado mover arana..\n");       
+    char nmotor=command.getc();
+    char pos=command.getc();
+    char endc=command.getc();
+    mover_ser2(nmotor,pos); 
+    debug_m("fin del comado guardar..\n");    
+}
+
+void moving3(){
+   
+    debug_m("se inicia el comado caminar adelante..\n");    
+    mover_ser3();
+    debug_m("fin del comado guardar..\n");    
+}
+
+
+void sensor()
+{
+    while(1){
+        color=command.getc();        
+                S0=1;
+                S1=1; 
+                S2=0;
+                S3=0;                
+                rojo=Detectar();
+                command.printf("ROJO ");
+                command.printf("%d  ",rojo);
+                S0=1;
+                S1=1; 
+                S2=0;
+                S3=1;
+                azul=Detectar();
+                command.printf("AZUL ");
+                command.printf("%d   ",azul);
+                S0=1;
+                S1=1;             
+                S2=1;
+                S3=1;
+                verde=Detectar();
+                command.printf("VERDE");
+                command.printf("%d\n   ",verde);
+                
+                
+                if ((rojo>600)&(rojo<800) & (azul>600)&(azul<800) & (verde>100)){
+                    
+                    command.printf("AZUL \n");}
+                    
+                    else{
+                        if ((rojo<200) & (azul<200) & (verde<200)){
+                            
+                            command.printf("ROJO\n");}
+                            
+                            else{
+                                
+                        if ((rojo>500)&(rojo<800) & (azul>500)&(azul<800) & (verde<100)){
+                            
+                            command.printf("VERDE\n");}
+                            
+                            else{
+                                command.printf("COLOR NO VALIDO\n");
+                                }
+                                }
+                            }
+   }
+   }
+    
+    int Detectar(){
+    tme=0;
+    while (!entrada){}
+    while (entrada) {}
+    while (!entrada){}
+    tiempo.start();
+    while (entrada) {}
+    while (!entrada){}
+    while (entrada) {}
+    while (!entrada){}
+    while (entrada) {}
+    tiempo.stop();
+    tme=tiempo.read_us();
+    tiempo.reset();
+    return(tme);
+    
+    
+    }
+
+
+
+void debug_m(char *s , ... ){
+    #if DEBUG
+    command.printf(s);
+    #endif  
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Thu Nov 22 01:19:55 2018 +0000
@@ -0,0 +1,20 @@
+#ifndef MAIN_H   
+#define MAIN_H  
+#include "mover.h"
+
+//******************************************************************************
+// Desarrollado por ferney beltran fbeltran@ecci.edu.co
+//******************************************************************************
+
+#define DEBUG 1
+
+void debug_m(char *s , ... );
+uint32_t read_command();
+void init_serial();
+
+void moving();
+void moving2();
+void moving3();
+void sensor();
+
+#endif //  MAIN_H 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 22 01:19:55 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mover.cpp	Thu Nov 22 01:19:55 2018 +0000
@@ -0,0 +1,251 @@
+#include "mover.h"
+#include "mbed.h"
+#include "math.h"
+
+PwmOut myServo1(PB_13);//DFA
+PwmOut myServo2(PB_14);//DFB
+PwmOut myServo3(PB_1);//DTA
+PwmOut myServo4(PB_9);//DTB
+PwmOut myServo5(PB_2);//IFA
+PwmOut myServo6(PA_11);//IFB
+PwmOut myServo7(PC_6);//ITA
+PwmOut myServo8(PC_8);//ITB
+
+
+  
+
+uint8_t  ss_time= 255;     // 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(750+coord*1900/180);// u6
+    return 750;
+
+}
+
+
+ 
+
+
+void mover_ser(uint8_t motor, uint8_t pos){
+     int pulseX = coord2us(pos);
+     switch(motor){
+         
+    case 1:        
+    
+    myServo1.pulsewidth_us(pulseX) ; 
+  
+    
+    break;
+    
+    case 2:        
+    
+    myServo2.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 3:        
+    
+    myServo3.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 4:        
+    
+    myServo4.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 5:        
+    
+    myServo5.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 6:  myServo6.pulsewidth_us(pulseX); break;
+    
+    case 7:        
+    
+    myServo7.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 8:        
+    
+    myServo8.pulsewidth_us(pulseX);
+    
+    break;
+    }
+         
+     
+
+}
+
+void init_servo()
+{
+   myServo1.period_ms(20);
+   myServo2.period_ms(20);
+   myServo3.period_ms(20);
+   myServo4.period_ms(20);
+   myServo5.period_ms(20);
+   myServo6.period_ms(20);
+   myServo7.period_ms(20);
+   myServo8.period_ms(20);
+
+}
+void mover_ser2(uint8_t nmotor, uint8_t pos){
+     switch(nmotor){
+    case 1:        
+    if(pos==1){
+    myServo1.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo1.pulsewidth_us(1500);
+    }
+    else if(pos==3){
+    myServo2.pulsewidth_us(2650);
+    }
+    else if(pos==4){
+    myServo2.pulsewidth_us(1600);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    case 2:        
+    
+    if(pos==1){
+    myServo3.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo3.pulsewidth_us(908);
+    }
+    else if(pos==3){
+    myServo4.pulsewidth_us(2650);
+    }
+    else if(pos==4){
+    myServo4.pulsewidth_us(1600);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    case 3:        
+    
+   if(pos==1){
+    myServo5.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo5.pulsewidth_us(1200);
+    }
+    else if(pos==3){
+    myServo6.pulsewidth_us(908);
+    }
+    else if(pos==4){
+    myServo6.pulsewidth_us(1600);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    case 4:        
+    
+    if(pos==1){
+    myServo7.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo7.pulsewidth_us(908);
+    }
+    else if(pos==3){
+    myServo8.pulsewidth_us(908);
+    }
+    else if(pos==4){
+    myServo8.pulsewidth_us(1800);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    }
+    }
+     void mover_ser3(){
+        myServo1.pulsewidth_us(600);
+        myServo3.pulsewidth_us(2500);
+        myServo5.pulsewidth_us(2500);
+        myServo7.pulsewidth_us(700);
+    
+
+       
+        myServo2.pulsewidth_us(1400);
+        myServo4.pulsewidth_us(2400);
+        myServo6.pulsewidth_us(1700);
+        myServo8.pulsewidth_us(300);
+        wait(1);
+        myServo1.pulsewidth_us(1500);
+        wait(1);        
+        myServo2.pulsewidth_us(700);
+        wait(1);  
+        myServo1.pulsewidth_us(600);
+        wait(1);
+        myServo5.pulsewidth_us(1600);
+        wait(1);
+        myServo6.pulsewidth_us(2400);
+        wait(1);
+        myServo5.pulsewidth_us(2500);
+        
+        wait(1);
+        myServo7.pulsewidth_us(1700);
+        wait(1);
+        myServo8.pulsewidth_us(1400);
+        wait(1);
+        myServo7.pulsewidth_us(700);
+        
+        wait(1);
+        myServo3.pulsewidth_us(1600);
+        wait(1);
+        myServo4.pulsewidth_us(1700);
+        wait(1);
+        myServo3.pulsewidth_us(2500);
+        wait(1);
+        myServo2.pulsewidth_us(1400);
+        myServo4.pulsewidth_us(2400);
+        myServo6.pulsewidth_us(1700);
+        myServo8.pulsewidth_us(300);
+        
+        wait(1);
+     
+       
+   
+               
+        }
+    
+ 
+      
+    
+
+        
+        
+        
+        
+        
+        
+        
+        
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mover.h	Thu Nov 22 01:19:55 2018 +0000
@@ -0,0 +1,18 @@
+#ifndef DRAW_H   
+#define DRAW_H  
+
+#include "mbed.h"
+
+#define MAXPOS 180       // en milimetros
+ 
+
+
+void init_servo();
+
+void mover_ser(uint8_t motor, uint8_t pos);
+void mover_ser2(uint8_t motor, uint8_t pos);
+void mover_ser3();
+
+
+
+#endif //  DRAW_H 
\ No newline at end of file