Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Diff: PCConnector/SWUSBServer.cpp

Revision:
17:84acc292b4c2
Parent:
16:66c7a09e71ee
Child:
18:bf6c5f8281eb
--- a/PCConnector/SWUSBServer.cpp	Fri Feb 10 03:23:20 2017 +0000
+++ b/PCConnector/SWUSBServer.cpp	Fri Feb 10 16:17:57 2017 +0000
@@ -49,6 +49,7 @@
     
     if(!m_shouldTerminate && m_stat == SER_STAT_CONNECTED)
     {
+        PushReliableMsg('D', "Terminal Connected!");
         if(m_usbThread)
         {
             delete m_usbThread;
@@ -57,8 +58,10 @@
         //m_hidThread.start(callback(this, &USBServer::HIDThread));
     }
     
+    //char buf[15];
     if(m_tickMsgTimer >= 2.0f)
     {
+        //sprintf(buf, "%s,%f", TICKING_MSG, m_tickMsgTimer);
         PushReliableMsg('T', TICKING_MSG);
         m_tickMsgTimer = 0.0f;
     }
@@ -134,9 +137,32 @@
     
     m_usb = new Serial(USBTX, USBRX, "SmartWheels");
     
-    m_usb->printf("\x02%s\x03", HANDSHAKE_MSG_TER);
-    
-    m_stat = SER_STAT_CONNECTED;
+    while(m_stat != SER_STAT_CONNECTED)
+    {
+        
+        while(!m_usb->readable())
+        {
+            m_usb->printf("\x02%s\x03", HANDSHAKE_MSG_TER);
+            wait(1.0);
+        }
+        
+        char chBuf = m_usb->getc();
+        if(chBuf == '\x02')
+        {
+            std::string strBuf;
+            while(chBuf != '\x03')
+            {
+                chBuf = m_usb->getc();
+                if(chBuf != '\x03')
+                    strBuf.push_back(chBuf);
+            }
+            
+            if(strBuf.compare(HANDSHAKE_MSG_PC) == 0)
+            {
+                m_stat = SER_STAT_CONNECTED;
+            }
+        }
+    }
     
 }