Diff: main.cpp

Revision:

0:10bd8964965a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 05 08:16:30 2016 +0000
@@ -0,0 +1,152 @@
+#include "mbed.h"
+#include "LidarSpi.h"
+#include "LidarFunc.h"
+#include "USBSerial.h"
+#include "tests.h"
+//DigitalOut myled(LED1);
+
+/*Protocol:
+SEND: 
+    "CMD PAR1 PAR2\n"
+RETURN
+    status(S) nrOfResponseBytes(L) Response(R)
+    SLLLLR..R
+*/
+
+Serial pc(USBTX, USBRX);
+USBSerial serial;
+
+LocalFileSystem local("local"); 
+
+int main() {
+
+    LidarSpi lidar(p5, p6, p7, p8, p15, p21, p22, p23);
+    LidarFunc sens(&lidar);
+    
+    char cmd[10];
+    char msg[50];
+    uint32_t par1=0;
+    uint32_t par2=0;
+    int res;
+    int cnt=0;
+    int n;
+    uint32_t size;
+    uint32_t val;
+    uint8_t * pntrS;
+    uint8_t * pntrV;
+    pc.baud(115200);
+    while(1){
+      
+        pc.printf("Awaiting commands...\n\r");
+
+        do{
+            msg[cnt]=serial.getc();
+            cnt++;
+            msg[cnt]='\0';
+            //pc.printf("getc:  %s", msg);
+        } while (msg[cnt-1]!='\n' && cnt<50);
+        cnt=0;
+        pc.printf("getc:  %s\n\r", msg);
+        n = sscanf(msg,
+                           "%9[^' '] %x %x\n", 
+                           cmd,
+                           &par1,
+                           &par2
+                        );
+        
+    
+        
+        if((strcmp(cmd, "RREG")==0) && n==2){  //ReadRegister Address
+            pc.printf("ReadRegister, address: %x", par1);
+            
+            res=lidar.ReadReg(par1, &val);
+            val=(val>>16)&0xffff;
+            
+            
+            if(res<0){
+                size=0x0000;
+                pntrS=(uint8_t *)&size;
+                serial.putc(1);
+                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
+                pc.printf("Read Error\n\r");
+            }
+            else{
+                size=sizeof(val);
+                pntrS=(uint8_t *) &size;
+                pntrV=(uint8_t *) &val;
+                pc.printf("size: %d\n\r", *pntrS);
+                serial.putc(0);
+                for(int i=0;i<sizeof(size);i++) serial.putc(*(pntrS+i));
+                for(int i=0;i<sizeof(size);i++) pc.printf("putc Val: [%d] %d\n\r",i,pntrV[i]);
+                for(int i=0;i<sizeof(val);i++) serial.putc(*(pntrV+i));
+                pc.printf("Address Value: %x  ,   sizeof(val)= %d\n\r", *pntrV, sizeof(val));
+                
+            }
+            
+    
+        }
+        else if((strcmp(cmd, "WREG")==0) && n==3){  //WriteRegister Address Value
+            pc.printf("WriteRegister, address: %x, value:%x", par1, par2);
+            
+            res=lidar.WriteReg(par1, par2);
+            if(res<0){
+
+                size=0;
+                pntrS=(uint8_t *)&size;
+                serial.putc(1);
+                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
+            }
+            else{
+  
+                size=0;
+                pntrS=(uint8_t *)&size;
+                serial.putc(0);
+                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
+            }
+    
+        }
+        else if((strcmp(cmd, "PTCH")==0) && n==1){  //WriteRegister Address Value
+            pc.printf("LoadPatch\n\r");
+            
+            res=lidar.LoadPatch("/local/patch.hex", &pc);
+            if(res<0){
+
+                size=0;
+                pntrS=(uint8_t *)&size;
+                serial.putc(1);
+                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
+                pc.printf("Patch load failed\n\r");
+            }
+            else{
+  
+                size=0;
+                pntrS=(uint8_t *)&size;
+                serial.putc(0);
+                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
+                pc.printf("Patch load Success\n\r");
+            }
+    
+        }
+        else if((strcmp(cmd, "TRCE")==0) && n==2){  //WriteRegister Address Value
+            pc.printf("getTrace\n\r");
+            bool isEnabled;
+            if (par1>0) isEnabled=true;
+            else isEnabled=false;
+            res=sens.setLed(isEnabled);
+            res=lidar.setTrace();
+            res=traceTest(&lidar, &pc, &serial);
+        }
+        else
+        {
+            pc.printf("INVALID COMMAND\n\r");
+            size=0;
+            pntrS=(uint8_t *)&size;
+            serial.putc(1);
+            for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]); 
+        }
+    }
+    
+    
+    
+    
+}
\ No newline at end of file