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@11:676ea42afd56, 2017-02-07 (annotated)

Committer:
Bobymicjohn
Date:
Tue Feb 07 21:58:20 2017 +0000
Revision:
11:676ea42afd56
Child:
13:7dcb1642ef99
Finished Core, and Servo classes.

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),
Bobymicjohn 11:676ea42afd56 13 m_stat(HID_SER_STAT_STOPPED),
Bobymicjohn 11:676ea42afd56 14 m_usbThread(NULL),
Bobymicjohn 11:676ea42afd56 15 m_usb(NULL)
Bobymicjohn 11:676ea42afd56 16 {
Bobymicjohn 11:676ea42afd56 17
Bobymicjohn 11:676ea42afd56 18 }
Bobymicjohn 11:676ea42afd56 19
Bobymicjohn 11:676ea42afd56 20 USBServer::~USBServer()
Bobymicjohn 11:676ea42afd56 21 {
Bobymicjohn 11:676ea42afd56 22 Terminate();
Bobymicjohn 11:676ea42afd56 23 }
Bobymicjohn 11:676ea42afd56 24
Bobymicjohn 11:676ea42afd56 25 void USBServer::Terminate()
Bobymicjohn 11:676ea42afd56 26 {
Bobymicjohn 11:676ea42afd56 27 m_shouldTerminate = true;
Bobymicjohn 11:676ea42afd56 28 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 29 m_usbThread->signal_set(0xffff);
Bobymicjohn 11:676ea42afd56 30 //m_usbThread->join();
Bobymicjohn 11:676ea42afd56 31 m_usbThread->terminate();
Bobymicjohn 11:676ea42afd56 32 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 33 delete m_usb;
Bobymicjohn 11:676ea42afd56 34 }
Bobymicjohn 11:676ea42afd56 35
Bobymicjohn 11:676ea42afd56 36 void USBServer::Update(float deltaTime)
Bobymicjohn 11:676ea42afd56 37 {
Bobymicjohn 11:676ea42afd56 38
Bobymicjohn 11:676ea42afd56 39 if(!m_shouldTerminate && m_stat == HID_SER_STAT_STOPPED)
Bobymicjohn 11:676ea42afd56 40 {
Bobymicjohn 11:676ea42afd56 41 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 42 {
Bobymicjohn 11:676ea42afd56 43 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 44 }
Bobymicjohn 11:676ea42afd56 45 m_usbThread = new Thread(callback(this, &USBServer::ConnectingThread));
Bobymicjohn 11:676ea42afd56 46 //m_hidThread.start(callback(this, &USBServer::HIDConnectingThread));
Bobymicjohn 11:676ea42afd56 47 }
Bobymicjohn 11:676ea42afd56 48
Bobymicjohn 11:676ea42afd56 49 if(!m_shouldTerminate && m_stat == HID_SER_STAT_CONNECTED)
Bobymicjohn 11:676ea42afd56 50 {
Bobymicjohn 11:676ea42afd56 51 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 52 {
Bobymicjohn 11:676ea42afd56 53 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 54 }
Bobymicjohn 11:676ea42afd56 55 m_usbThread = new Thread(callback(this, &USBServer::RunningThread));
Bobymicjohn 11:676ea42afd56 56 //m_hidThread.start(callback(this, &USBServer::HIDThread));
Bobymicjohn 11:676ea42afd56 57 }
Bobymicjohn 11:676ea42afd56 58 }
Bobymicjohn 11:676ea42afd56 59
Bobymicjohn 11:676ea42afd56 60 bool USBServer::PushReliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 61 {
Bobymicjohn 11:676ea42afd56 62 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 63 {
Bobymicjohn 11:676ea42afd56 64 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 65 m_msgQueue.push_back(type + msg);
Bobymicjohn 11:676ea42afd56 66 if(m_stat == HID_SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 67 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 68 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 69 return true;
Bobymicjohn 11:676ea42afd56 70 }
Bobymicjohn 11:676ea42afd56 71 else
Bobymicjohn 11:676ea42afd56 72 {
Bobymicjohn 11:676ea42afd56 73 return false;
Bobymicjohn 11:676ea42afd56 74 }
Bobymicjohn 11:676ea42afd56 75 }
Bobymicjohn 11:676ea42afd56 76
Bobymicjohn 11:676ea42afd56 77 bool USBServer::PushUnreliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 78 {
Bobymicjohn 11:676ea42afd56 79 if(m_stat != HID_SER_STAT_RUNNING || m_msgQueue.size() >= UNRELIABLE_QUEUE_MAX_SIZE)
Bobymicjohn 11:676ea42afd56 80 return false;
Bobymicjohn 11:676ea42afd56 81
Bobymicjohn 11:676ea42afd56 82 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 83 {
Bobymicjohn 11:676ea42afd56 84 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 85 m_msgQueue.push_back(type + msg);
Bobymicjohn 11:676ea42afd56 86 if(m_stat == HID_SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 87 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 88 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 89 return true;
Bobymicjohn 11:676ea42afd56 90 }
Bobymicjohn 11:676ea42afd56 91 else
Bobymicjohn 11:676ea42afd56 92 {
Bobymicjohn 11:676ea42afd56 93 return false;
Bobymicjohn 11:676ea42afd56 94 }
Bobymicjohn 11:676ea42afd56 95 }
Bobymicjohn 11:676ea42afd56 96
Bobymicjohn 11:676ea42afd56 97 void USBServer::RunningThread()
Bobymicjohn 11:676ea42afd56 98 {
Bobymicjohn 11:676ea42afd56 99 m_stat = HID_SER_STAT_RUNNING;
Bobymicjohn 11:676ea42afd56 100
Bobymicjohn 11:676ea42afd56 101 while(!m_shouldTerminate)
Bobymicjohn 11:676ea42afd56 102 {
Bobymicjohn 11:676ea42afd56 103 //m_qlocker.lock(); m_uqlocker.lock();
Bobymicjohn 11:676ea42afd56 104 if(m_msgQueue.size() <= 0)
Bobymicjohn 11:676ea42afd56 105 {
Bobymicjohn 11:676ea42afd56 106 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 107 Thread::signal_wait(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 108 }
Bobymicjohn 11:676ea42afd56 109 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 110 std::string msg = "";
Bobymicjohn 11:676ea42afd56 111 if(m_msgQueue.size() > 0)
Bobymicjohn 11:676ea42afd56 112 {
Bobymicjohn 11:676ea42afd56 113 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 114 msg = m_msgQueue.front();
Bobymicjohn 11:676ea42afd56 115 m_msgQueue.pop_front();
Bobymicjohn 11:676ea42afd56 116 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 117 }
Bobymicjohn 11:676ea42afd56 118
Bobymicjohn 11:676ea42afd56 119 m_usb->printf("%s\n", msg.c_str());
Bobymicjohn 11:676ea42afd56 120 }
Bobymicjohn 11:676ea42afd56 121 }
Bobymicjohn 11:676ea42afd56 122
Bobymicjohn 11:676ea42afd56 123 void USBServer::ConnectingThread()
Bobymicjohn 11:676ea42afd56 124 {
Bobymicjohn 11:676ea42afd56 125 m_stat = HID_SER_STAT_CONNECTING;
Bobymicjohn 11:676ea42afd56 126
Bobymicjohn 11:676ea42afd56 127 m_usb = new Serial(USBTX, USBRX, "SmartWheels");
Bobymicjohn 11:676ea42afd56 128
Bobymicjohn 11:676ea42afd56 129 m_usb->printf("%s\n", HANDSHAKE_MSG_TER);
Bobymicjohn 11:676ea42afd56 130
Bobymicjohn 11:676ea42afd56 131 m_stat = HID_SER_STAT_CONNECTED;
Bobymicjohn 11:676ea42afd56 132
Bobymicjohn 11:676ea42afd56 133 }
Bobymicjohn 11:676ea42afd56 134
Bobymicjohn 11:676ea42afd56 135 }