control brazo robotico

Dependencies:   mbed

Revision:
0:58800e1db02a
diff -r 000000000000 -r 58800e1db02a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat May 11 03:22:07 2019 +0000
@@ -0,0 +1,420 @@
+#include "mbed.h"
+#include "scolor_TCS3200.h"
+ 
+/*********
+Este archivo es una modificacion al ejercicio propuesta por el ingeniero Ferney alberto Beltran
+molina. 
+generar un programa que controle por el puerto serial el grado de 4 servo motores.
+por medio de la comunicacion serial el comando es 
+ 
+|            |                 |             |               |
+|   INITCMD  |   Tipo_Comando  | Parametro_1 | Funcion       |
+|     0xff   |        0x01     |    0x01     | Pos. Home     |
+|     0xff   |        0x01     |    0x02     | Pos. Material |
+|     0xff   |        0x01     |    0x03     | Pos. Celda 1  |
+|     0xff   |        0x01     |    0x04     | Pos. Celda 2  |
+|     0xff   |        0x01     |    0x05     | Pos. Celda 3  |
+|     0xff   |        0x01     |    0x06     | Pos. Celda 4  |
+|     0xff   |        0x02     |    0x01     | Abre Pinza    |
+|     0xff   |        0x02     |    0x02     | Cierra Pinza  |
+|     0xff   |        0x03     | 0x00 - 0xb4 | Mueve Motor 1 |
+|     0xff   |        0x04     | 0x00 - 0xb4 | Mueve Motor 2 |
+|     0xff   |        0x05     | 0x00 - 0xb4 | Mueve Motor 3 |
+|     0xff   |        0x06     | 0x00 - 0xb4 | Mueve Motor 4 |
+|     0xff   |        0x07     | 0x00 - 0x03 | Mueve Motor 1 |
+|     0xff   |        0x08     | 0x00 - 0xb4 | Mueve Motor 2 |
+|     0xff   |        0x09     | 0x00 - 0xb4 | Mueve Motor 3 |
+ 
+ 
+para enviar los comandos usar el programa Coolterm http://freeware.the-meiers.org/
+ 
+# para el servo motor se debe modificar el ciclo util del PWM SEGUN:
+#     _              _
+# |        |_|        |___
+#    <-width->
+#    <-------period 20ms--->
+# period = 20 ms
+# width  = 1000us y 2000us
+#  1000us para 0 grados
+#  2000us para 180 grados
+_________
+SO | S1 | OUTPUT FREQUENCY SCALING |        | S2 | S3 |   PHOTODIODE TYPE   |
+ 0 | 0  |        power down        |        |  0 | 0  |         Red         |
+ 0 | 1  |           2%             |        |  0 | 1  |         Blue        |
+ 1 | 0  |           20%            |        |  1 | 0  |  Clear (no filter)  |
+ 1 | 1  |           100%           |        |  1 | 1  |         Green       | 
+             
+ 
+*********/
+ 
+// configuracion comunicación serial 
+Serial command(USBTX, USBRX);
+// seleccion de los pines a usar en la stm32f411re
+PwmOut myservo1(PB_4);
+PwmOut myservo2(PB_3);
+PwmOut myservo3(PB_10);
+PwmOut myservo4(PC_7);
+scolor_TCS3200 scolor(PA_8, PA_9, PB_6, PA_7, PA_6);
+AnalogIn analog_value1(A1);
+AnalogIn analog_value2(A2);
+AnalogIn analog_value3(A3);
+DigitalIn abrir(PA_10);
+DigitalIn cerrar(PB_5);
+ 
+ 
+ 
+ 
+ 
+ 
+#define INITCMD 0xFF
+#define INITELE 0xFE
+#define DEGREES_MAX 180
+#define CMD  0x01 
+ 
+// definición de las variables globales 
+ 
+ 
+uint8_t Tipo_Comando; // varable que almacena el numero de seleccion de comando
+uint8_t Parametro_1; // varable que almacena el parametro a ejecutar
+uint8_t velociraptor =10 ;
+uint32_t dpulse=0;
+ 
+ 
+// definición de las funciones
+void setup_uart();
+void setup_servo();
+void leer_datos();
+void leer_color(); 
+void servo(uint8_t T_C, uint8_t Par_1);
+uint32_t degrees2usec(uint8_t grados);
+uint8_t cmd;
+    
+    
+int main() {
+ 
+ setup_servo();
+ setup_uart();
+ abrir.mode(PullUp);
+cerrar.mode(PullUp);
+    
+ while(1){
+    leer_datos();
+    servo(Tipo_Comando, Parametro_1);
+    
+    }    
+}
+ 
+ 
+ 
+ 
+ 
+void setup_uart(){
+    command.baud(115200);
+}
+ 
+void setup_servo(){
+    myservo1.period_ms(20);
+    myservo1.pulsewidth_us(1350);
+    myservo2.period_ms(20);
+    myservo2.pulsewidth_us(900);
+    myservo3.period_ms(20);
+    myservo3.pulsewidth_us(1900);
+    myservo4.period_ms(20);
+    myservo4.pulsewidth_us(1550);
+}
+ 
+// funcion leer_datos obtiene los comandos enviados desde cool Terminal 
+void leer_datos(){
+ while(command.getc()!= INITCMD);
+    Tipo_Comando=command.getc();
+    Parametro_1=command.getc();
+    
+}
+ 
+ 
+//Esta funcion convierte de grados a microsengudos, segun el ancho de pulso del servomotor
+uint32_t degrees2usec(uint8_t grados){
+ if(grados <= DEGREES_MAX)
+    return float(440+grados*17.33/2);// u6
+ return 440; 
+}
+ 
+ 
+void servo(uint8_t T_C, uint8_t Par_1){
+ 
+ dpulse=degrees2usec(Par_1);
+ 
+    switch(T_C){
+        case 1:
+                if (Par_1==1){//posicion home
+                    myservo3.pulsewidth_us(degrees2usec(180));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(110));
+
+ 
+                     }
+                if (Par_1==2){//posicion material
+                     
+                      myservo3.pulsewidth_us(degrees2usec(180));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     
+                     
+                
+                     myservo3.pulsewidth_us(degrees2usec(70));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(170));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                      
+                    }
+                if (Par_1==3){//posicion celda 1
+                
+                
+                     myservo3.pulsewidth_us(degrees2usec(180));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     
+                     
+                
+               
+                     myservo1.pulsewidth_us(degrees2usec(35));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     myservo3.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+
+                     }
+                if (Par_1==4){//posicion celda 2
+                
+                
+                     myservo3.pulsewidth_us(degrees2usec(180));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     
+                     
+                
+                      
+                     myservo1.pulsewidth_us(degrees2usec(70));
+                     wait(1);
+                      myservo2.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     myservo3.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+
+                     }
+                if (Par_1==5){//posicion celda 3
+                 
+                     myservo3.pulsewidth_us(degrees2usec(180));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                    
+                
+                
+                    
+                     myservo1.pulsewidth_us(degrees2usec(105));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     myservo3.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+
+                     
+                     }
+                if (Par_1==6){//posicion celda 4
+                
+                
+              
+                    myservo3.pulsewidth_us(degrees2usec(180));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(50));
+                     wait(1);
+                     myservo1.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                    
+                
+                     
+                     myservo1.pulsewidth_us(degrees2usec(140));
+                     wait(1);
+                     myservo2.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     myservo3.pulsewidth_us(degrees2usec(90));
+                     wait(1);
+                     
+
+                     }
+            break;
+                
+        case 2:
+                if (Par_1==1){//abrir pinza
+                     myservo4.pulsewidth_us(degrees2usec(100));
+                     }
+                     
+                if (Par_1==2){//cerrar pinza
+                     myservo4.pulsewidth_us(degrees2usec(170));
+                     }  
+            break;
+               
+        case 3:
+            myservo1.pulsewidth_us(dpulse);
+  
+                
+            break;
+                 
+        case 4:
+               myservo2.pulsewidth_us(dpulse);
+  
+            break;
+        case 5:
+              myservo3.pulsewidth_us(dpulse);
+  
+            break;
+        case 6:
+                if(Par_1>=120)
+               myservo4.pulsewidth_us(dpulse);
+
+            break;
+ 
+        case 7:
+                if(Par_1==1){//velocida baja
+                  myservo1.period_ms(40);
+                  myservo1.pulsewidth_us(1350);
+                  myservo2.period_ms(40);
+                  myservo2.pulsewidth_us(900);
+                  myservo3.period_ms(40);
+                  myservo3.pulsewidth_us(1900);
+                  myservo4.period_ms(40);
+                  myservo4.pulsewidth_us(1550);                         
+                }
+                
+                else if(Par_1==2){//velocidad media
+                  myservo1.period_ms(20);
+                  myservo1.pulsewidth_us(1350);
+                  myservo2.period_ms(20);
+                  myservo2.pulsewidth_us(900);
+                  myservo3.period_ms(20);
+                  myservo3.pulsewidth_us(1900);
+                  myservo4.period_ms(20);
+                  myservo4.pulsewidth_us(1550); 
+                }
+                
+                else{ //velocidad alta
+                  myservo1.period_ms(10);
+                  myservo1.pulsewidth_us(1350);
+                  myservo2.period_ms(10);
+                  myservo2.pulsewidth_us(900);
+                  myservo3.period_ms(10);
+                  myservo3.pulsewidth_us(1900);
+                  myservo4.period_ms(10);
+                  myservo4.pulsewidth_us(1550); 
+                }      
+                 
+            break;
+        case 8:
+            float meas_r1,meas_r2,meas_r3;
+            float meas_m1,meas_m2, meas_m3;
+            float grado1, grado2 , grado3;
+            bool x=false;
+            if(Par_1==1){
+ 
+            while(!x){
+                    
+                    
+            meas_r1 = analog_value1.read(); // Read the analog input value (value from 0.0 to 1.0 = full ADC conversion range)
+            meas_r2 = analog_value2.read();
+            meas_r3 = analog_value3.read();
+               
+            meas_m1 = (meas_r1 * 180) ; // Converts value in the 0V-3.3V range
+            meas_m2 = (meas_r2 * 180) ;
+            meas_m3 = (meas_r3 * 180) ;        
+            
+           
+            if(meas_m1<= 180){
+                grado1=(440+(meas_m1+20)*20.33/2);// u6
+                myservo1.pulsewidth_us(grado1);}
+                  else{ myservo1.pulsewidth_us(440);}
+                
+             if(meas_m2>55){
+                grado2=(440+(meas_m2-40)*20.33/2);// u6
+                myservo2.pulsewidth_us(grado2); 
+                }
+                else{ myservo2.pulsewidth_us(1300);}    
+                 
+                 
+                   
+                if(meas_m3<= 55){
+                  grado3=(440+(meas_m3+30)*20.33/2);// u6
+                  myservo3.pulsewidth_us(grado3);}
+                      else{ myservo3.pulsewidth_us(1900);}
+                
+                if(!abrir){myservo4.pulsewidth_us(degrees2usec(120));}
+                
+                if(!cerrar){myservo4.pulsewidth_us(degrees2usec(170));}
+                
+                if(command.readable()){
+                    leer_datos();
+                    servo(Tipo_Comando, Parametro_1); 
+                    if(Par_1==2){
+                        break;}
+                    }
+        
+ 
+            wait(0.05); // 1 second
+            
+            }}
+            
+            
+             
+        case 9:
+            long     red;
+            long     green;
+            long     blue;
+            long     clear;
+            
+            red = scolor.ReadRed();
+            green = scolor.ReadGreen();
+            blue = scolor.ReadBlue();
+            clear = scolor.ReadClear();
+            
+            if(red<green && red<blue){
+            
+            if (red<8){
+            command.printf("0xFE 0x04 \n ");
+                                    }
+                                  else{  
+                                   command.printf("0xFE 0x01 \n ");}
+                                    
+                                    }
+            
+            if(green<red && green<blue){
+            command.printf("0xFE 0x03 \n ");
+                                    }
+            if(blue<green && blue<red){
+            command.printf("0xFE 0x02 \n ");
+                                    }
+                                    
+
+ 
+          //  command.printf("RED: %5d     GREEN: %5d     BLUE: %5d     CLEAR: %5d    \n ", red, green, blue, clear);}
+            break;
+                    
+    
+}}
\ No newline at end of file