Diff: main.cpp

Revision:

0:a8e3f4b25a52

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 09 11:59:23 2017 +0000
@@ -0,0 +1,212 @@
+#include "mbed.h"
+#include "string.h"
+#include "command_decoder.h"
+#include "servo.h"
+
+DigitalOut led(LED1);
+DigitalOut led2(LED2); 
+
+Serial pc(USBTX, USBRX);
+
+//================================================
+//          PUT_STRING
+//================================================
+
+int puts( char * tab , int size )
+{
+    int ret_val;
+    for ( int i = 0; i < size; i++)
+    {
+        if(tab[i] == NULL)
+        {
+            ret_val = 0;
+            tab[i]  = '\r';
+            break;
+        }
+        else
+        {
+            ret_val = 1;
+        }
+    }
+    
+    if(ret_val == 0)  
+    {     
+        while(*tab != '\r')
+        {
+            if(pc.writeable())
+                {
+                    pc.putc((int)(*tab));
+                    tab++;
+                }
+        }
+        pc.putc((int)(*tab));
+    }
+    else
+    {
+        //empty else
+    }
+    
+    
+    return ret_val;
+}
+
+//================================================
+//          GET_STRING
+//================================================
+
+int gets( char * tab , int size )
+{
+   int Counter = 0, ret_val = 0;
+   while((*tab = pc.getc()) != '\r')
+   {
+       if(Counter < size)
+       {
+           
+       }
+       else
+       {
+           ret_val = 1;
+       }
+       Counter++;
+       tab++;
+   }
+   
+   *tab = NULL;
+   
+   if(ret_val)
+      while(pc.readable())
+      {
+          pc.getc();
+      }
+      
+  return ret_val;
+}
+
+//================================================
+//          INT_TO_CHAR
+//================================================
+
+void IntToChar ( int number, char * wynik)
+{
+    // zakres do tysiecy
+    int temp = 0;
+    
+    for (int i = 3 ; i >= 0; i--)
+    {
+        temp = number % 10;
+        wynik [i] = 48 + temp;
+        number = number - temp;
+        number = number / 10;
+    }
+}
+
+//================================================
+//          COMPARE_STRINGS
+//================================================
+
+int compstr( char * a, char * b)
+{
+   
+    while(*b != NULL )
+        if(*a == *b)
+        {
+            a++;
+            b++;
+        }  
+        else
+        return 0;
+    return 1;
+}
+
+//================================================
+//          ToLower
+//================================================
+
+void ToLower(char * chain, int size)
+{
+    for(int i = 0; i < size; i++)
+    {
+        if(chain[i] >= 65 && chain[i] <= 90)
+        {
+            chain[i] = chain[i] + 32;
+        }
+        else
+        {
+            //empty else
+        }
+    }
+}
+
+//================================================
+//          ClearTab
+//================================================
+
+void ClearTab(char * tab, int size)
+{
+    for(int i = 0; i < size; i++)
+        tab[i] = NULL;
+}
+
+void FillTab(char * tab, const char * data, int size)
+{
+    ClearTab(tab, size);
+    while(*data != NULL)
+    {
+        *tab = *data;
+        data++;
+        tab++;
+    }
+    *tab = *data;
+        
+}
+
+extern unsigned char ucTokenNr;
+extern struct Token asToken[MAX_TOKEN_NR];
+
+
+int main()
+{
+
+    char Receive [24];
+    Servo_Init(10);
+    while(1) {
+    
+       if( gets(Receive,24) == 0 )
+       {
+            DecodeMsg(Receive);
+                
+            {
+                switch(asToken[0].uValue.eKeyword){
+                    case ID:
+                        FillTab(Receive, "id 3234", 24);
+                        puts(Receive,24);
+                        break;
+                    case CALLIB:
+                        Servo_Callib();
+                        FillTab(Receive, "callib_ok", 24);
+                        puts(Receive,24);
+                        led = !led;
+                        break;
+                    case GOTO:
+                        Servo_GoTo(asToken[1].uValue.uiNumber);
+                        FillTab(Receive, "goto_ok", 24);
+                        puts(Receive,24);
+                        led = !led;
+                        break;
+                    case STEP:
+                        Servo_Step(asToken[1].uValue.uiNumber);
+                        FillTab(Receive, "step_ok", 24);
+                        puts(Receive,24);
+                        led = !led;
+                        break;
+                    default:
+                        FillTab(Receive, "unknowncommend", 24);
+                        puts(Receive,24);
+                        break;
+                    }
+                }
+         }
+    }   
+}
+
+