uart-nozen: baudrate fix eb_robobagr: no change

eb_smeatk: inital upload

diff --git a/src/robobagr/main.c b/src/robobagr/main.c
index 6453e10f54f4df63d9b744ef192ec1703a64e7d9..66ed3f5976b45a6b90d027c79671b8ea31bc6a82 100644 (file)
--- a/src/robobagr/main.c
+++ b/src/robobagr/main.c
@@ -9,6 +9,7 @@
 #include <servo.h>
 #include <powswitch.h>
 #include <engine.h>
+#include <adc.h>
 #include <uart_nozen.h>
 
 
@@ -26,6 +27,23 @@
 
 can_msg_t msg;
 
+unsigned int cmu_buff[30];
+unsigned int cmu_buff_p = 0;
+
+struct Color{
+  unsigned char red;
+  unsigned char green;
+  unsigned char blue;
+  unsigned char d_red;
+  unsigned char d_green;
+  unsigned char d_blue;        
+} ;
+
+struct Color color;
+
+
+
+
 
 void dummy_wait(void)
 {
@@ -83,7 +101,11 @@ void init_perip(void)          // inicializace periferii mikroprocesoru
        init_pow_switch();
 
        init_engine_A();
-       init_engine_B();
+       engine_A_en(ENGINE_EN_ON);
+       engine_A_dir(ENGINE_DIR_FW);
+       engine_A_pwm(20);
+       pow_switch_on();
+       //init_engine_B();
 
        UART_init( UART0,UART0_ISR, 19200, UART_BITS_8, UART_STOP_BIT_1,  UART_PARIT_OFF, 0);
        UART_init( UART1,UART1_ISR, 115100, UART_BITS_8, UART_STOP_BIT_1,  UART_PARIT_OFF, 0);
@@ -108,6 +130,70 @@ int cmu_init(void)
 
 }
 
+void cmu_parse_message(void)
+{
+       unsigned int val = 0;
+       unsigned int i = 0;
+       unsigned int iter = 1;
+       unsigned int colors[6];
+
+
+
+       for (i = 0; i < 6; i++)
+       {
+               colors[i] = 0;
+       }
+
+       
+
+       if (cmu_buff[0] != 'S') return;
+       if (cmu_buff[1] != ' ') return;
+
+
+       
+       for ( val = cmu_buff_p ; val > 0 ; --val)
+       {
+               
+               if ((cmu_buff[val] >= '0' ) & (cmu_buff[val] <= '9' )) break;
+                       
+               
+       }
+
+
+       for (i =0; i < 6 ;i++)
+       {
+               iter = 1;
+
+               for ( ; val > 0  ; --val)
+               {
+                       if (cmu_buff[val] >= '0')
+                       {                               
+                               colors[i] += (cmu_buff[val] - '0') * iter ;
+                               iter *= 10;
+                       }
+                       else 
+                       {                               
+                               --val;
+                               break;
+                       }
+               }
+       }
+
+
+
+
+
+       color.red = colors[5];
+       color.green = colors[4];
+       color.blue =  colors[3];
+       color.d_red = colors[2];
+       color.d_green =colors[1];
+       color.d_blue =colors[0];        
+}
+
+
+
+
 void cmu_set_param(void)
 {
        char param[] = "CR 18 44\r";
@@ -120,8 +206,6 @@ void cmu_set_param(void)
 
 }
 
-char cmu_buff[30];
-int cmu_buff_p = 0;
 
 int cmu_ack(void)
 {
@@ -138,9 +222,10 @@ int cmu_ack(void)
                        int i =0;
                        for ( i = 0; i <= cmu_buff_p ; i++)
                        {                               
-                               if (cmu_buff[i] == 'N') return 2;
-                               if (cmu_buff[i] == 'A') return 1;
+                               if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;}
+                               if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;}
                        }
+                       cmu_buff_p = 0;
                        return 2;       
                }
                ++cmu_buff_p;
@@ -163,7 +248,6 @@ void cmu_set_gm(void)
 
 int cmu_ack_gm()
 {
-       
        if (cmu_timeout < time_ms) return 2;
        while (UART_new_data(UART1))
        {
@@ -178,10 +262,10 @@ int cmu_ack_gm()
                        {                               
                                write_UART_data( UART0, cmu_buff[i]);
                
-                               if (cmu_buff[i] == 'N') return 2;
-                               if (cmu_buff[i] == 'A') return 1;
+                               if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;}
+                               if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;}
                        }
-                       
+                       cmu_buff_p = 0;
                        return 2;       
                }
                ++cmu_buff_p;
@@ -191,32 +275,53 @@ int cmu_ack_gm()
 }
 
 
+void atoim(unsigned int val)
+{
+       write_UART_data( UART0, ' ' );
+       write_UART_data( UART0, (val / 100 + 48) );
+       write_UART_data( UART0, ((val % 100) / 10 + 48) );
+       write_UART_data( UART0, ((val % 100) % 10 + 48) );
+       
+}
+
 
-void cmu_gm(void)
+int cmu_gm(void)
 {
        int test;
        
-//     test = UART_test_err(UART1);
-// 
-//     if(test == -1)
-//     {
-//             deb_led_on(LEDY);
-//             while(1);
-//     }       
-//             
-//     if(test == -2)
-//     {
-//             deb_led_on(LEDB);
-//             deb_led_on(LEDY);
-//             while(1);
-//     }
 
        if (UART_new_data(UART1))
        {
                test = read_UART_data(UART1);
-               write_UART_data( UART0, test);
+               if (test == '\r') 
+               {       
                
+                       if (cmu_buff_p < 13)   // if recieved chars is not full message
+                       {                               
+                               cmu_buff_p = 0;
+                               //return 2;     
+                       }
+
+                       cmu_parse_message();
+
+                       atoim(color.red );
+                       atoim(color.green );
+                       atoim(color.blue );
+                       atoim(color.d_red );
+                       atoim(color.d_green );
+                       atoim(color.d_blue );
+                       write_UART_data( UART0, '\n');
+
+                       cmu_buff_p = 0;
+               }
+               else 
+               {       
+                       cmu_buff[cmu_buff_p] = test;    
+                       write_UART_data( UART0, cmu_buff[cmu_buff_p]);
+                       ++cmu_buff_p;
+               }
        }
+       return 0;
        
 }
 
@@ -240,15 +345,6 @@ int main (void)  {
        set_servo(0, 0x80);
        set_servo(1, 0x80);
        set_servo(2, 0x80);
-
-       engine_A_dir(ENGINE_DIR_BW);//FW);
-       engine_B_dir(ENGINE_DIR_FW);
-       engine_A_en(ENGINE_EN_ON);
-       engine_B_en(ENGINE_EN_ON);
-       engine_A_pwm(70);
-       engine_B_pwm(70);
-
-
        
        int state = 0; 
        int ret =0;
@@ -263,7 +359,13 @@ int main (void)  {
        while(1)
        {                  
 
-       //      write_UART_data( UART0,  48 + state);
+
+               if (UART_new_data(UART0))
+               {
+                       engine_A_en(ENGINE_EN_OFF);
+                       engine_A_pwm(0);
+               }
+
 
                led_blik();
 
@@ -281,7 +383,7 @@ int main (void)  {
                                cmu_set_param();
                                state = 2;
                                cmu_timeout = time_ms + 400;
-                               cmu_buff_p = 0;
+                               //cmu_buff_p = 0;
                                break;
 
                        case 2:
@@ -295,7 +397,7 @@ int main (void)  {
                                cmu_set_gm();
                                state = 4;
                                cmu_timeout = time_ms + 400;
-                               cmu_buff_p = 0;
+                               //cmu_buff_p = 0;
                                break;
 
                        case 4:
@@ -304,7 +406,10 @@ int main (void)  {
                                if (ret ==  2) state = 9;
                                break;
 
-                       case 5: cmu_gm();       
+                       case 5: 
+                               ret = cmu_gm();
+                               deb_led_on(LEDY);
+                               if(ret == 2) state = 9; 
                                break;
 
 
@@ -336,5 +441,19 @@ int main (void)  {
                {
                        write_UART_data( UART1, read_UART_data(UART0));
                }*/
+//     test = UART_test_err(UART1);
+// 
+//     if(test == -1)
+//     {
+//             deb_led_on(LEDY);
+//             while(1);
+//     }       
+//             
+//     if(test == -2)
+//     {
+//             deb_led_on(LEDB);
+//             deb_led_on(LEDY);
+//             while(1);
+//     }