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

PCConnector/SWUSBServer.cpp@17:84acc292b4c2, 2017-02-10 (annotated)

Committer:
hazheng
Date:
Fri Feb 10 16:17:57 2017 +0000
Revision:
17:84acc292b4c2
Parent:
16:66c7a09e71ee
Child:
18:bf6c5f8281eb
Finished the connecting part.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bobymicjohn 11:676ea42afd56 1 #include "SWUSBServer.h"
Bobymicjohn 11:676ea42afd56 2
Bobymicjohn 11:676ea42afd56 3 #define THREAD_SIGNAL_QUEUE 0xa
Bobymicjohn 11:676ea42afd56 4 #define UNRELIABLE_QUEUE_MAX_SIZE 5
Bobymicjohn 11:676ea42afd56 5
Bobymicjohn 11:676ea42afd56 6 #include "USBSerial.h"
Bobymicjohn 11:676ea42afd56 7
Bobymicjohn 11:676ea42afd56 8 namespace SW{
Bobymicjohn 11:676ea42afd56 9
Bobymicjohn 11:676ea42afd56 10 USBServer::USBServer(uint16_t vendor_id, uint16_t product_id) :
Bobymicjohn 11:676ea42afd56 11 //m_hid(HID_REPORT_LENGTH, HID_REPORT_LENGTH, vendor_id, product_id),
Bobymicjohn 11:676ea42afd56 12 m_shouldTerminate(false),
hazheng 13:7dcb1642ef99 13 m_stat(SER_STAT_STOPPED),
Bobymicjohn 11:676ea42afd56 14 m_usbThread(NULL),
hazheng 16:66c7a09e71ee 15 m_usb(NULL),
hazheng 16:66c7a09e71ee 16 m_tickMsgTimer(0.0f)
Bobymicjohn 11:676ea42afd56 17 {
Bobymicjohn 11:676ea42afd56 18
Bobymicjohn 11:676ea42afd56 19 }
Bobymicjohn 11:676ea42afd56 20
Bobymicjohn 11:676ea42afd56 21 USBServer::~USBServer()
Bobymicjohn 11:676ea42afd56 22 {
Bobymicjohn 11:676ea42afd56 23 Terminate();
Bobymicjohn 11:676ea42afd56 24 }
Bobymicjohn 11:676ea42afd56 25
Bobymicjohn 11:676ea42afd56 26 void USBServer::Terminate()
Bobymicjohn 11:676ea42afd56 27 {
Bobymicjohn 11:676ea42afd56 28 m_shouldTerminate = true;
Bobymicjohn 11:676ea42afd56 29 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 30 m_usbThread->signal_set(0xffff);
Bobymicjohn 11:676ea42afd56 31 //m_usbThread->join();
Bobymicjohn 11:676ea42afd56 32 m_usbThread->terminate();
Bobymicjohn 11:676ea42afd56 33 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 34 delete m_usb;
Bobymicjohn 11:676ea42afd56 35 }
Bobymicjohn 11:676ea42afd56 36
Bobymicjohn 11:676ea42afd56 37 void USBServer::Update(float deltaTime)
Bobymicjohn 11:676ea42afd56 38 {
hazheng 16:66c7a09e71ee 39 m_tickMsgTimer += deltaTime;
hazheng 13:7dcb1642ef99 40 if(!m_shouldTerminate && m_stat == SER_STAT_STOPPED)
Bobymicjohn 11:676ea42afd56 41 {
Bobymicjohn 11:676ea42afd56 42 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 43 {
Bobymicjohn 11:676ea42afd56 44 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 45 }
Bobymicjohn 11:676ea42afd56 46 m_usbThread = new Thread(callback(this, &USBServer::ConnectingThread));
Bobymicjohn 11:676ea42afd56 47 //m_hidThread.start(callback(this, &USBServer::HIDConnectingThread));
Bobymicjohn 11:676ea42afd56 48 }
Bobymicjohn 11:676ea42afd56 49
hazheng 13:7dcb1642ef99 50 if(!m_shouldTerminate && m_stat == SER_STAT_CONNECTED)
Bobymicjohn 11:676ea42afd56 51 {
hazheng 17:84acc292b4c2 52 PushReliableMsg('D', "Terminal Connected!");
Bobymicjohn 11:676ea42afd56 53 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 54 {
Bobymicjohn 11:676ea42afd56 55 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 56 }
Bobymicjohn 11:676ea42afd56 57 m_usbThread = new Thread(callback(this, &USBServer::RunningThread));
Bobymicjohn 11:676ea42afd56 58 //m_hidThread.start(callback(this, &USBServer::HIDThread));
Bobymicjohn 11:676ea42afd56 59 }
hazheng 16:66c7a09e71ee 60
hazheng 17:84acc292b4c2 61 //char buf[15];
hazheng 16:66c7a09e71ee 62 if(m_tickMsgTimer >= 2.0f)
hazheng 16:66c7a09e71ee 63 {
hazheng 17:84acc292b4c2 64 //sprintf(buf, "%s,%f", TICKING_MSG, m_tickMsgTimer);
hazheng 16:66c7a09e71ee 65 PushReliableMsg('T', TICKING_MSG);
hazheng 16:66c7a09e71ee 66 m_tickMsgTimer = 0.0f;
hazheng 16:66c7a09e71ee 67 }
Bobymicjohn 11:676ea42afd56 68 }
Bobymicjohn 11:676ea42afd56 69
Bobymicjohn 11:676ea42afd56 70 bool USBServer::PushReliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 71 {
Bobymicjohn 11:676ea42afd56 72 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 73 {
Bobymicjohn 11:676ea42afd56 74 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 75 m_msgQueue.push_back(type + msg);
hazheng 13:7dcb1642ef99 76 if(m_stat == SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 77 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 78 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 79 return true;
Bobymicjohn 11:676ea42afd56 80 }
Bobymicjohn 11:676ea42afd56 81 else
Bobymicjohn 11:676ea42afd56 82 {
Bobymicjohn 11:676ea42afd56 83 return false;
Bobymicjohn 11:676ea42afd56 84 }
Bobymicjohn 11:676ea42afd56 85 }
Bobymicjohn 11:676ea42afd56 86
Bobymicjohn 11:676ea42afd56 87 bool USBServer::PushUnreliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 88 {
hazheng 13:7dcb1642ef99 89 if(m_stat != SER_STAT_RUNNING || m_msgQueue.size() >= UNRELIABLE_QUEUE_MAX_SIZE)
Bobymicjohn 11:676ea42afd56 90 return false;
Bobymicjohn 11:676ea42afd56 91
Bobymicjohn 11:676ea42afd56 92 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 93 {
Bobymicjohn 11:676ea42afd56 94 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 95 m_msgQueue.push_back(type + msg);
hazheng 13:7dcb1642ef99 96 if(m_stat == SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 97 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 98 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 99 return true;
Bobymicjohn 11:676ea42afd56 100 }
Bobymicjohn 11:676ea42afd56 101 else
Bobymicjohn 11:676ea42afd56 102 {
Bobymicjohn 11:676ea42afd56 103 return false;
Bobymicjohn 11:676ea42afd56 104 }
Bobymicjohn 11:676ea42afd56 105 }
Bobymicjohn 11:676ea42afd56 106
Bobymicjohn 11:676ea42afd56 107 void USBServer::RunningThread()
Bobymicjohn 11:676ea42afd56 108 {
hazheng 13:7dcb1642ef99 109 m_stat = SER_STAT_RUNNING;
Bobymicjohn 11:676ea42afd56 110
Bobymicjohn 11:676ea42afd56 111 while(!m_shouldTerminate)
Bobymicjohn 11:676ea42afd56 112 {
Bobymicjohn 11:676ea42afd56 113 //m_qlocker.lock(); m_uqlocker.lock();
Bobymicjohn 11:676ea42afd56 114 if(m_msgQueue.size() <= 0)
Bobymicjohn 11:676ea42afd56 115 {
Bobymicjohn 11:676ea42afd56 116 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 117 Thread::signal_wait(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 118 }
Bobymicjohn 11:676ea42afd56 119 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 120 std::string msg = "";
Bobymicjohn 11:676ea42afd56 121 if(m_msgQueue.size() > 0)
Bobymicjohn 11:676ea42afd56 122 {
Bobymicjohn 11:676ea42afd56 123 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 124 msg = m_msgQueue.front();
Bobymicjohn 11:676ea42afd56 125 m_msgQueue.pop_front();
Bobymicjohn 11:676ea42afd56 126 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 127 }
Bobymicjohn 11:676ea42afd56 128
Bobymicjohn 11:676ea42afd56 129 m_usb->printf("%s\n", msg.c_str());
Bobymicjohn 11:676ea42afd56 130 }
hazheng 16:66c7a09e71ee 131
Bobymicjohn 11:676ea42afd56 132 }
Bobymicjohn 11:676ea42afd56 133
Bobymicjohn 11:676ea42afd56 134 void USBServer::ConnectingThread()
Bobymicjohn 11:676ea42afd56 135 {
hazheng 13:7dcb1642ef99 136 m_stat = SER_STAT_CONNECTING;
Bobymicjohn 11:676ea42afd56 137
Bobymicjohn 11:676ea42afd56 138 m_usb = new Serial(USBTX, USBRX, "SmartWheels");
Bobymicjohn 11:676ea42afd56 139
hazheng 17:84acc292b4c2 140 while(m_stat != SER_STAT_CONNECTED)
hazheng 17:84acc292b4c2 141 {
hazheng 17:84acc292b4c2 142
hazheng 17:84acc292b4c2 143 while(!m_usb->readable())
hazheng 17:84acc292b4c2 144 {
hazheng 17:84acc292b4c2 145 m_usb->printf("\x02%s\x03", HANDSHAKE_MSG_TER);
hazheng 17:84acc292b4c2 146 wait(1.0);
hazheng 17:84acc292b4c2 147 }
hazheng 17:84acc292b4c2 148
hazheng 17:84acc292b4c2 149 char chBuf = m_usb->getc();
hazheng 17:84acc292b4c2 150 if(chBuf == '\x02')
hazheng 17:84acc292b4c2 151 {
hazheng 17:84acc292b4c2 152 std::string strBuf;
hazheng 17:84acc292b4c2 153 while(chBuf != '\x03')
hazheng 17:84acc292b4c2 154 {
hazheng 17:84acc292b4c2 155 chBuf = m_usb->getc();
hazheng 17:84acc292b4c2 156 if(chBuf != '\x03')
hazheng 17:84acc292b4c2 157 strBuf.push_back(chBuf);
hazheng 17:84acc292b4c2 158 }
hazheng 17:84acc292b4c2 159
hazheng 17:84acc292b4c2 160 if(strBuf.compare(HANDSHAKE_MSG_PC) == 0)
hazheng 17:84acc292b4c2 161 {
hazheng 17:84acc292b4c2 162 m_stat = SER_STAT_CONNECTED;
hazheng 17:84acc292b4c2 163 }
hazheng 17:84acc292b4c2 164 }
hazheng 17:84acc292b4c2 165 }
Bobymicjohn 11:676ea42afd56 166
Bobymicjohn 11:676ea42afd56 167 }
Bobymicjohn 11:676ea42afd56 168
hazheng 13:7dcb1642ef99 169
hazheng 13:7dcb1642ef99 170 uint8_t USBServer::GetStatus() const
hazheng 13:7dcb1642ef99 171 {
hazheng 13:7dcb1642ef99 172 return m_stat;
hazheng 13:7dcb1642ef99 173 }
hazheng 13:7dcb1642ef99 174
Bobymicjohn 11:676ea42afd56 175 }