Revision 0:58800e1db02a, committed 2019-05-11

Comitter:

mrdarka94

Date:

Sat May 11 03:22:07 2019 +0000

Commit message:

lol

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat May 11 03:22:07 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/scolor_TCS3200.cpp	Sat May 11 03:22:07 2019 +0000
@@ -0,0 +1,40 @@
+#include "mbed.h"
+#include "scolor_TCS3200.h"
+
+
+scolor_TCS3200::scolor_TCS3200(PinName s0, PinName s1, PinName s2, PinName s3, PinName s_in) : 
+_s0(s0), _s1(s1), _s2(s2), _s3(s3), _s_in(s_in) 
+{
+    SetMode(SCALE_100);
+
+};
+ 
+
+long scolor_TCS3200::ReadRed()   { _s2=0;  _s3=0; return pulsewidth();}
+long scolor_TCS3200::ReadBlue()  { _s2=0;  _s3=1; return pulsewidth();}
+long scolor_TCS3200::ReadClear() { _s2=1;  _s3=0; return pulsewidth();}
+long scolor_TCS3200::ReadGreen() { _s2=1;  _s3=1; return pulsewidth();}
+
+void scolor_TCS3200::SetMode(uint8_t mode) {
+    switch (mode){
+        case SCALE_100:  _s0= 1; _s1=1; break;
+        case SCALE_20:   _s0=1 ; _s1=0; break;
+        case SCALE_2:    _s0=0 ; _s1=1; break;
+        case POWER_DOWN: _s0=0 ; _s1=0; break;
+    } 
+};
+ 
+long  scolor_TCS3200::pulsewidth() {
+    while(_s_in);
+    while(!_s_in);
+    timer.start();
+    while(_s_in);
+    while(!_s_in);
+    timer.stop();
+    float pulsewidth_v = timer.read_us();
+    timer.reset();
+    return pulsewidth_v;
+};
+
+            
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/scolor_TCS3200.h	Sat May 11 03:22:07 2019 +0000
@@ -0,0 +1,63 @@
+#ifndef SCOLOR_TCS3200_H
+#define SCOLOR_TCS3200_H
+#include "mbed.h"
+ /* **************************************************************************
+ 
+@fabeltranm 2019
+fbeltranm@ecci.edu.co
+
+
+   datasheet https://www.mouser.com/catalog/specsheets/TCS3200-E11.pdf
+
+
+    S0      Frequency scaling 
+    S1      Frequency scaling 
+    S2      Photo diode selection 
+    S3      Photo diode selection 
+    OutFreq Frequency
+
+       -----------------------------------
+      |   ____________     ____________   |
+----> |  |            |   |            |  |                ___     ___ 
+Light |  | Photodiode |   |   Current  |--|---OUTPUT_FREQ |   |___|   |___
+----> |  |   Array    |---|     to     |  |
+      |  |            |   |  Frequency |  |
+      |  |____________|   |____________|  |
+      |       ^  ^             ^  ^       |
+       -------|--|-------------|--|-------
+              |  |             |  |
+             S2 S3            S0  S1
+             
+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       | 
+             
+******************************************************************************/
+
+
+#define SCALE_100   1
+#define SCALE_20    2
+#define SCALE_2     3    
+#define POWER_DOWN  4
+
+class scolor_TCS3200 {
+  public:
+    scolor_TCS3200(PinName s0, PinName s1, PinName s2, PinName s3, PinName s_in);
+    long ReadRed();     // retorno el tiempo en alto de OutFreq para Rojo en ns
+    long ReadGreen();   // retorno el tiempo en alto de OutFreq para verde en ns
+    long ReadBlue();    // retorno el tiempo en alto de OutFreq color azul en ns
+    long ReadClear();   // retorno el tiempo en alto de OutFreq sin filtro en ns
+    void SetMode(uint8_t mode);
+  private:
+    DigitalOut _s0;
+    DigitalOut _s1;
+    DigitalOut _s2;
+    DigitalOut _s3;
+    DigitalIn _s_in;
+    Timer timer;
+    long pulsewidth();
+
+};
+#endif
\ No newline at end of file