mbed-rtos test programs for DISCO_F746NG. #Test for thread, mutex, semaphore, signals, queues, mail, ISR

Dependencies:   BSP_DISCO_F746NG HC06Bluetooth I2Cdev LCD_DISCO_F746NG MCP2515 MD25 MPU9150 TS_DISCO_F746NG emwin mbed-dev mbed-rtos pca9685 ppCANOpen ros_lib_kinetic

Fork of DISCO-F746NG_rtos_test by sabme ua

Files at this revision

API Documentation at this revision

Comitter:
TuxLeon
Date:
Sun Feb 11 18:41:01 2018 +0000
Parent:
0:225c1da086a1
Commit message:
first commit;

Changed in this revision

BSP_DISCO_F746NG.lib Show annotated file Show diff for this revision Revisions of this file
HC06Bluetooth.lib Show annotated file Show diff for this revision Revisions of this file
I2Cdev.lib Show annotated file Show diff for this revision Revisions of this file
LCD_DISCO_F746NG.lib Show annotated file Show diff for this revision Revisions of this file
MCP2515.lib Show annotated file Show diff for this revision Revisions of this file
MD25.lib Show annotated file Show diff for this revision Revisions of this file
MPU9150.lib Show annotated file Show diff for this revision Revisions of this file
TS_DISCO_F746NG.lib Show annotated file Show diff for this revision Revisions of this file
emwin.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-dev.lib Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
mbed-src.lib Show diff for this revision Revisions of this file
pca9685.lib Show annotated file Show diff for this revision Revisions of this file
ppCANOpen.lib Show annotated file Show diff for this revision Revisions of this file
ros_lib_kinetic.lib Show annotated file Show diff for this revision Revisions of this file
test_env.cpp Show diff for this revision Revisions of this file
test_env.h Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BSP_DISCO_F746NG.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TuxLeon/code/BSP_DISCO_F746NG/#99b5a7505a7a
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HC06Bluetooth.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Electrotiger/code/HC06Bluetooth/#cce827364df1
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2Cdev.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TuxLeon/code/I2Cdev/#df0c2ace5932
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LCD_DISCO_F746NG.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/LCD_DISCO_F746NG/#d44525b1de98
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MCP2515.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/FalconOnishi/code/MCP2515/#b65cb1bbf0ce
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MD25.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/jimherd/code/MD25/#e575d390c730
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9150.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TuxLeon/code/MPU9150/#c38eb550c82e
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TS_DISCO_F746NG.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/TS_DISCO_F746NG/#fe0cf5e2960f
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/emwin.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/redbird/code/emwin/#1bf8f02b0770
--- a/main.cpp	Wed Nov 11 07:53:46 2015 +0000
+++ b/main.cpp	Sun Feb 11 18:41:01 2018 +0000
@@ -1,591 +1,177 @@
 #include "mbed.h"
-#include "test_env.h"
-#include "rtos.h"
-
-/********************
- * Switch test case
- ********************/
-#define TEST_84_BASIC
-//#define TEST_85_MUTEX
-//#define TEST_86_SEMAPHORE
-//#define TEST_87_SIGNALS
-//#define TEST_88_QUEUE
-//#define TEST_89_MAIL
-//#define TEST_90_TIMER
-//#define TEST_91_ISR
-
-#if defined(TEST_84_BASIC)
-
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if (defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)) && defined(TOOLCHAIN_GCC)
-#define STACK_SIZE DEFAULT_STACK_SIZE/2
-#elif (defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB)) && defined(TOOLCHAIN_GCC)
-#define STACK_SIZE DEFAULT_STACK_SIZE/2
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_IAR)
-#define STACK_SIZE DEFAULT_STACK_SIZE/2
-#else
-#define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
-
-void print_char(char c = '*') {
-    printf("%c", c);
-    fflush(stdout);
-}
-
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-
-void led2_thread(void const *argument) {
-    while (true) {
-        led2 = !led2;
-        Thread::wait(1000);
-        print_char();
-    }
-}
-
-int main() {
-    MBED_HOSTTEST_TIMEOUT(15);
-    MBED_HOSTTEST_SELECT(wait_us_auto);
-    MBED_HOSTTEST_DESCRIPTION(Basic thread);
-    MBED_HOSTTEST_START("RTOS_1");
-
-    Thread thread(led2_thread, NULL, osPriorityNormal, STACK_SIZE);
-
-    while (true) {
-        led1 = !led1;
-        Thread::wait(500);
-    }
-}
-
-#elif defined(TEST_85_MUTEX)
-
-#define THREAD_DELAY     50
-#define SIGNALS_TO_EMIT  100
-
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if (defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif (defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif defined(TARGET_STM32F334R8) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif defined(TARGET_STM32F030R8) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4 
-#elif defined(TARGET_STM32F070RB) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2 
-#elif defined(TARGET_STM32F072RB) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2 
-#elif defined(TARGET_STM32F302R8) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2     
-#else
-    #define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
-
-void print_char(char c = '*') {
-    printf("%c", c);
-    fflush(stdout);
-}
+#include "TS_DISCO_F746NG.h"
+#include "LCD_DISCO_F746NG.h"
+#include "pca9685.h"
+#include "I2Cdev.h"
+#include "MPU9150.h"
+#include "PinNames.h"
 
-Mutex stdio_mutex;
-DigitalOut led(LED1);
-
-volatile int change_counter = 0;
-volatile bool changing_counter = false;
-volatile bool mutex_defect = false;
-
-bool manipulate_protected_zone(const int thread_delay) {
-    bool result = true;
-
-    stdio_mutex.lock(); // LOCK
-    if (changing_counter == true) {
-        // 'e' stands for error. If changing_counter is true access is not exclusively
-        print_char('e');
-        result = false;
-        mutex_defect = true;
-    }
-    changing_counter = true;
-
-    // Some action on protected
-    led = !led;
-    change_counter++;
-    print_char('.');
-    Thread::wait(thread_delay);
-
-    changing_counter = false;
-    stdio_mutex.unlock();   // UNLOCK
-    return result;
-}
-
-void test_thread(void const *args) {
-    const int thread_delay = int(args);
-    while (true) {
-        manipulate_protected_zone(thread_delay);
-    }
-}
-
-int main() {
-    MBED_HOSTTEST_TIMEOUT(20);
-    MBED_HOSTTEST_SELECT(default);
-    MBED_HOSTTEST_DESCRIPTION(Mutex resource lock);
-    MBED_HOSTTEST_START("RTOS_2");
-
-    const int t1_delay = THREAD_DELAY * 1;
-    const int t2_delay = THREAD_DELAY * 2;
-    const int t3_delay = THREAD_DELAY * 3;
-    Thread t2(test_thread, (void *)t2_delay, osPriorityNormal, STACK_SIZE);
-    Thread t3(test_thread, (void *)t3_delay, osPriorityNormal, STACK_SIZE);
+LCD_DISCO_F746NG lcd;
+TS_DISCO_F746NG ts;
+//I2C myI2C(I2C_SCL, I2C_SDA);
+//PCA9685 myPWM(1, &myI2C, 400000.0); 
+PwmOut led(PC_7);
+I2Cdev* myI2Cdev = new I2Cdev();
+MPU9150 myImu(*myI2Cdev);
+Serial pc(USBTX, USBRX);
 
-    while (true) {
-        // Thread 1 action
-        Thread::wait(t1_delay);
-        manipulate_protected_zone(t1_delay);
-        if (change_counter >= SIGNALS_TO_EMIT or mutex_defect == true) {
-            t2.terminate();
-            t3.terminate();
-            break;
-        }
-    }
-
-    fflush(stdout);
-    MBED_HOSTTEST_RESULT(!mutex_defect);
-    return 0;
-}
-
-#elif defined(TEST_86_SEMAPHORE)
-
-#define THREAD_DELAY     75
-#define SEMAPHORE_SLOTS  2
-#define SEM_CHANGES      100
-
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if (defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/16
-#elif (defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/8
-#elif defined(TARGET_STM32F334R8) && (defined(TOOLCHAIN_GCC) || defined(TOOLCHAIN_IAR))
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif defined(TARGET_STM32F103RB) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif defined(TARGET_STM32F030R8) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4 
-#elif defined(TARGET_STM32F070RB) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2 
-#elif defined(TARGET_STM32F072RB) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2 
-#elif defined(TARGET_STM32F302R8) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2     
-#else
-    #define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
-
-void print_char(char c = '*') {
-    printf("%c", c);
-    fflush(stdout);
+void callback() {
+    // Note: you need to actually read from the serial to clear the RX interrupt
+    pc.printf("%d\n", pc.getc());
 }
 
-#define ORIGINAL 1
-#undef ORIGINAL
 
-#if ORIGINAL
-Semaphore two_slots(SEMAPHORE_SLOTS);
-#else
-Semaphore *two_slots;
-#endif
-
-volatile int change_counter = 0;
-volatile int sem_counter = 0;
-volatile bool sem_defect = false;
+int main()
+{
+    pc.baud(115200);
+    pc.attach(&callback);
+    pc.printf("\n\nEEPROM demo started\n");
+    
+    TS_StateTypeDef TS_State;
+    uint16_t x, y;
+    uint8_t text[30];
+    uint8_t textColor[50];
+    uint8_t status;
+    uint8_t idx;
+    uint8_t cleared = 0;
+    uint8_t prev_nb_touches = 0;
+    uint8_t color_r = 0;
+    uint8_t color_g = 0;
+    uint8_t color_b = 128;
+    uint32_t color = 0xFF000000;
+    enum EColorState {RED, GREEN, BLUE};
+    float pwmOutput = 0.0;
+    EColorState eColorState = BLUE;
+    bool imuState = false;
 
-void test_thread(void const *delay) {
-    const int thread_delay = int(delay);
-    while (true) {
-#if ORIGINAL
-        two_slots.wait();
-#else
-        two_slots->wait();
-#endif
-        sem_counter++;
-        const bool sem_lock_failed = sem_counter > SEMAPHORE_SLOTS;
-        const char msg = sem_lock_failed ? 'e' : sem_counter + '0';
-        print_char(msg);
-        if (sem_lock_failed) {
-            sem_defect = true;
+    led.period_ms(20.0);      // 4 second period
+    //led.write(0.50f);      // 50% duty cycle, relative to period
+    led.pulsewidth_us(1500);
+    //myPWM.init();
+    //myPWM.set_pwm_duty(0, 50.0);
+    lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN DEMO", CENTER_MODE);
+    
+    wait(1);
+    
+    //myImu.initialize();
+    imuState = myImu.testConnection();
+
+    status = ts.Init(lcd.GetXSize(), lcd.GetYSize());
+    if (status != TS_OK) {
+        lcd.Clear(LCD_COLOR_RED);
+        lcd.SetBackColor(LCD_COLOR_RED);
+        lcd.SetTextColor(LCD_COLOR_WHITE);
+        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT FAIL", CENTER_MODE);
+    } else {
+        lcd.Clear(LCD_COLOR_GREEN);
+        lcd.SetBackColor(LCD_COLOR_GREEN);
+        lcd.SetTextColor(LCD_COLOR_WHITE);
+        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE);
+        if( imuState ) {
+            lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"IMU INIT OK", CENTER_MODE);
         }
-        Thread::wait(thread_delay);
-        print_char('.');
-        sem_counter--;
-        change_counter++;
-#if ORIGINAL
-        two_slots.release();
-#else
-        two_slots->release();
-#endif
-    }
-}
-
-int main (void) {
-
-#ifndef ORIGINAL
-    two_slots = new Semaphore(SEMAPHORE_SLOTS);
-#endif
-
-    MBED_HOSTTEST_TIMEOUT(20);
-    MBED_HOSTTEST_SELECT(default_auto);
-    MBED_HOSTTEST_DESCRIPTION(Semaphore resource lock);
-    MBED_HOSTTEST_START("RTOS_3");
-
-    const int t1_delay = THREAD_DELAY * 1;
-    const int t2_delay = THREAD_DELAY * 2;
-    const int t3_delay = THREAD_DELAY * 3;
-    Thread t1(test_thread, (void *)t1_delay, osPriorityNormal, STACK_SIZE);
-    Thread t2(test_thread, (void *)t2_delay, osPriorityNormal, STACK_SIZE);
-    Thread t3(test_thread, (void *)t3_delay, osPriorityNormal, STACK_SIZE);
-
-    while (true) {
-        if (change_counter >= SEM_CHANGES or sem_defect == true) {
-            t1.terminate();
-            t2.terminate();
-            t3.terminate();
-            break;
-        }
+        else{
+            lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"IMU INIT Failed", CENTER_MODE);
+        }    
     }
 
-    fflush(stdout);
-    MBED_HOSTTEST_RESULT(!sem_defect);
-    return 0;
-}
+    wait(1);
+    lcd.Clear(LCD_COLOR_WHITE);
+    lcd.SetFont(&Font12);
+    lcd.SetBackColor(LCD_COLOR_WHITE);
+    lcd.SetTextColor(LCD_COLOR_BLACK);
 
-#elif defined(TEST_87_SIGNALS)
-
-#define SIGNALS_TO_EMIT     100
-#define SIGNAL_HANDLE_DELEY 25
-#define SIGNAL_SET_VALUE    0x01
+    while(1) {
 
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2
-#else
-    #define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
+        ts.GetState(&TS_State);
+        if (TS_State.touchDetected) {
+            // Clear lines corresponding to old touches coordinates
+            if (TS_State.touchDetected < prev_nb_touches) {
+                for (idx = (TS_State.touchDetected + 1); idx <= 5; idx++) {
+                    lcd.ClearStringLine(idx);
+                }
+            }
+            prev_nb_touches = TS_State.touchDetected;
 
-DigitalOut led(LED1);
-volatile int signal_counter = 0;
+            cleared = 0;
 
-void led_thread(void const *argument) {
-    while (true) {
-        // Signal flags that are reported as event are automatically cleared.
-        Thread::signal_wait(SIGNAL_SET_VALUE);
-        led = !led;
-        signal_counter++;
-    }
-}
-
-int main (void) {
-    MBED_HOSTTEST_TIMEOUT(20);
-    MBED_HOSTTEST_SELECT(default_auto);
-    MBED_HOSTTEST_DESCRIPTION(Signals messaging);
-    MBED_HOSTTEST_START("RTOS_4");
-
-    Thread thread(led_thread, NULL, osPriorityNormal, STACK_SIZE);
-    bool result = true;
+            lcd.SetTextColor(LCD_COLOR_BLACK);
+            sprintf((char*)text, "Touches: %d", TS_State.touchDetected);
+            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);
 
-    while (true) {
-        Thread::wait(2 * SIGNAL_HANDLE_DELEY);
-        thread.signal_set(SIGNAL_SET_VALUE);
-        if (signal_counter == SIGNALS_TO_EMIT) {
-            printf("Handled %d signals\r\n", signal_counter);
-            break;
-        }
-    }
-    MBED_HOSTTEST_RESULT(result);
-    return 0;
-}
-
-#elif defined(TEST_88_QUEUE)
-
-typedef struct {
-    float    voltage;   /* AD result of measured voltage */
-    float    current;   /* AD result of measured current */
-    uint32_t counter;   /* A counter value               */
-} message_t;
+            for (idx = 0; idx < TS_State.touchDetected; idx++) {
+                x = TS_State.touchX[idx];
+                y = TS_State.touchY[idx];
+                sprintf((char*)text, "Touch %d: x=%d y=%d", idx+1, x, y);
+                lcd.DisplayStringAt(0, LINE(idx+1), (uint8_t *)&textColor, LEFT_MODE);
+                lcd.DisplayStringAt(0, LINE(idx+2), (uint8_t *)&text, LEFT_MODE);
+                
+          
+                pwmOutput = (float) (x/480.0);
+                if( pwmOutput > 1.0f )
+                {
+                    pwmOutput = 1.0f;
+                }
+                led.pulsewidth_us((int)(pwmOutput*2000 + 500));      // 50% duty cycle, relative to period
+                
 
-#define CREATE_VOLTAGE(COUNTER) (COUNTER * 0.1) * 33
-#define CREATE_CURRENT(COUNTER) (COUNTER * 0.1) * 11
-#define QUEUE_SIZE       16
-#define QUEUE_PUT_DELAY  100
-
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if (defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2
-#else
-    #define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
-
-MemoryPool<message_t, QUEUE_SIZE> mpool;
-Queue<message_t, QUEUE_SIZE> queue;
+                /**
+                * @brief  Sets the LCD text color.
+                * @param  Color: Text color code ARGB(8-8-8-8);
+                * @retval None
+                */
+                if( eColorState == BLUE ) {
+                    color_b++;
+                    if( color_b == 255 ) {
+                        eColorState = GREEN;
+                        color_b = 0;
+                        color_g = 128;
+                    }
+                }
+                if( eColorState == GREEN ) {
+                    color_g++;
+                    if( color_g == 255 ) {
+                        eColorState = RED;
+                        color_g = 0;
+                        color_r = 128;
+                    }
+                }
+                if( eColorState == RED ) {
+                    color_r++;
+                    if( color_r == 255 ) {
+                        eColorState = BLUE;
+                        color_r = 0;
+                        color_b = 128;
+                    }
+                }
 
-/* Send Thread */
-void send_thread (void const *argument) {
-    static uint32_t i = 10;
-    while (true) {
-        i++; // Fake data update
-        message_t *message = mpool.alloc();
-        message->voltage = CREATE_VOLTAGE(i);
-        message->current = CREATE_CURRENT(i);
-        message->counter = i;
-        queue.put(message);
-        Thread::wait(QUEUE_PUT_DELAY);
-    }
-}
+                for(uint8_t i=0; i <255; i++) {
+                    lcd.SetTextColor(0xFF000000 + i);
+                    lcd.FillCircle(i+100, 100, 5);
+                    lcd.SetTextColor(0xFF000000 + (i << 8));
+                    lcd.FillCircle(i+100, 150, 5);
+                    lcd.SetTextColor(0xFF000000 + (i << 16));
+                    lcd.FillCircle(i+100, 200, 5);
+                }
 
-int main (void) {
-    MBED_HOSTTEST_TIMEOUT(20);
-    MBED_HOSTTEST_SELECT(default_auto);
-    MBED_HOSTTEST_DESCRIPTION(Queue messaging);
-    MBED_HOSTTEST_START("RTOS_5");
+                color = 0xFF000000 + (color_r << 16) + (color_g << 8) + color_b;
+                sprintf((char*)textColor, "RED %x GREEN %x BLUE %x  Color=0x%x   ", color_r, color_g, color_b, color);
+                lcd.DisplayStringAt(0, LINE(idx+1), (uint8_t *)&textColor, LEFT_MODE);
 
-    Thread thread(send_thread, NULL, osPriorityNormal, STACK_SIZE);
-    bool result = true;
-    int result_counter = 0;
+                lcd.SetTextColor(color);
+                lcd.FillCircle(TS_State.touchX[0], TS_State.touchY[0], 5);
+            }
 
-    while (true) {
-        osEvent evt = queue.get();
-        if (evt.status == osEventMessage) {
-            message_t *message = (message_t*)evt.value.p;
-            const float expected_voltage = CREATE_VOLTAGE(message->counter);
-            const float expected_current = CREATE_CURRENT(message->counter);
-            // Check using macros if received values correspond to values sent via queue
-            bool expected_values = (expected_voltage == message->voltage) &&
-                                   (expected_current == message->current);
-            result = result && expected_values;
-            const char *result_msg = expected_values ? "OK" : "FAIL";
-            printf("%3d %.2fV %.2fA ... [%s]\r\n", message->counter,
-                                                   message->voltage,
-                                                   message->current,
-                                                   result_msg);
-            mpool.free(message);
-            if (result == false || ++result_counter == QUEUE_SIZE) {
-                break;
+            // lcd.DrawPixel(TS_State.touchX[0], TS_State.touchY[0], LCD_COLOR_ORANGE);
+        } else {
+            if (!cleared) {
+                
+                lcd.Clear(LCD_COLOR_WHITE);
+                sprintf((char*)text, "Touches: 0");
+                pc.printf("Servo: %f\n", pwmOutput*2000+500);
+                lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);
+                cleared = 1;
             }
         }
     }
-    MBED_HOSTTEST_RESULT(result);
-    return 0;
 }
-
-#elif defined(TEST_89_MAIL)
-
-typedef struct {
-    float    voltage;   /* AD result of measured voltage */
-    float    current;   /* AD result of measured current */
-    uint32_t counter;   /* A counter value               */
-} mail_t;
-
-#define CREATE_VOLTAGE(COUNTER) (COUNTER * 0.1) * 33
-#define CREATE_CURRENT(COUNTER) (COUNTER * 0.1) * 11
-#define QUEUE_SIZE       16
-#define QUEUE_PUT_DELAY  100
-
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if (defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_GCC)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2
-#else
-    #define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
-
-Mail<mail_t, QUEUE_SIZE> mail_box;
-
-void send_thread (void const *argument) {
-    static uint32_t i = 10;
-    while (true) {
-        i++; // fake data update
-        mail_t *mail = mail_box.alloc();
-        mail->voltage = CREATE_VOLTAGE(i);
-        mail->current = CREATE_CURRENT(i);
-        mail->counter = i;
-        mail_box.put(mail);
-        Thread::wait(QUEUE_PUT_DELAY);
-    }
-}
-
-int main (void) {
-    MBED_HOSTTEST_TIMEOUT(20);
-    MBED_HOSTTEST_SELECT(default_auto);
-    MBED_HOSTTEST_DESCRIPTION(Mail messaging);
-    MBED_HOSTTEST_START("RTOS_6");
-
-    Thread thread(send_thread, NULL, osPriorityNormal, STACK_SIZE);
-    bool result = true;
-    int result_counter = 0;
-
-    while (true) {
-        osEvent evt = mail_box.get();
-        if (evt.status == osEventMail) {
-            mail_t *mail = (mail_t*)evt.value.p;
-            const float expected_voltage = CREATE_VOLTAGE(mail->counter);
-            const float expected_current = CREATE_CURRENT(mail->counter);
-            // Check using macros if received values correspond to values sent via queue
-            bool expected_values = (expected_voltage == mail->voltage) &&
-                                   (expected_current == mail->current);
-            result = result && expected_values;
-            const char *result_msg = expected_values ? "OK" : "FAIL";
-            printf("%3d %.2fV %.2fA ... [%s]\r\n", mail->counter,
-                                                   mail->voltage,
-                                                   mail->current,
-                                                   result_msg);
-            mail_box.free(mail);
-            if (result == false || ++result_counter == QUEUE_SIZE) {
-                break;
-            }
-        }
-    }
-    MBED_HOSTTEST_RESULT(result);
-    return 0;
-}
-
-#elif defined(TEST_90_TIMER)
-
-DigitalOut LEDs[4] = {
-    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
-};
-
-void print_char(char c = '*')
-{
-    printf("%c", c);
-    fflush(stdout);
-}
-
-void blink(void const *n) {
-    static int counter = 0;
-    const int led_id = int(n);
-    LEDs[led_id] = !LEDs[led_id];
-    if (++counter == 75) {
-        print_char();
-        counter = 0;
-    }
-}
-
-int main(void) {
-    MBED_HOSTTEST_TIMEOUT(15);
-    MBED_HOSTTEST_SELECT(wait_us_auto);
-    MBED_HOSTTEST_DESCRIPTION(Timer);
-    MBED_HOSTTEST_START("RTOS_7");
-
-    RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
-    RtosTimer led_2_timer(blink, osTimerPeriodic, (void *)1);
-    RtosTimer led_3_timer(blink, osTimerPeriodic, (void *)2);
-    RtosTimer led_4_timer(blink, osTimerPeriodic, (void *)3);
-
-    led_1_timer.start(200);
-    led_2_timer.start(100);
-    led_3_timer.start(50);
-    led_4_timer.start(25);
-
-    Thread::wait(osWaitForever);
-}
-
-#elif defined(TEST_91_ISR)
-
-#define QUEUE_SIZE              5
-#define THREAD_DELAY            250
-#define QUEUE_PUT_ISR_VALUE     128
-#define QUEUE_PUT_THREAD_VALUE  127
-
-/*
- * The stack size is defined in cmsis_os.h mainly dependent on the underlying toolchain and
- * the C standard library. For GCC, ARM_STD and IAR it is defined with a size of 2048 bytes
- * and for ARM_MICRO 512. Because of reduce RAM size some targets need a reduced stacksize.
- */
-#if defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/4
-#elif (defined(TARGET_STM32F030R8)) && defined(TOOLCHAIN_IAR)
-    #define STACK_SIZE DEFAULT_STACK_SIZE/2
-#else
-    #define STACK_SIZE DEFAULT_STACK_SIZE
-#endif
-
-Queue<uint32_t, QUEUE_SIZE> queue;
-
-DigitalOut myled(LED1);
-
-void queue_isr() {
-
-    queue.put((uint32_t*)QUEUE_PUT_ISR_VALUE);
-    myled = !myled;
-}
-
-void queue_thread(void const *argument) {
-    while (true) {
-        queue.put((uint32_t*)QUEUE_PUT_THREAD_VALUE);
-        Thread::wait(THREAD_DELAY);
-    }
-}
-
-int main (void) {
-    MBED_HOSTTEST_TIMEOUT(20);
-    MBED_HOSTTEST_SELECT(default_auto);
-    MBED_HOSTTEST_DESCRIPTION(ISR (Queue));
-    MBED_HOSTTEST_START("RTOS_8");
-
-    Thread thread(queue_thread, NULL, osPriorityNormal, STACK_SIZE);
-    Ticker ticker;
-    ticker.attach(queue_isr, 1.0);
-    int isr_puts_counter = 0;
-    bool result = true;
-
-    while (true) {
-        osEvent evt = queue.get();
-        if (evt.status != osEventMessage) {
-            printf("QUEUE_GET: Status(0x%02X) ... [FAIL]\r\n", evt.status);
-            result = false;
-            break;
-        } else {
-            printf("QUEUE_GET: Value(%u) ... [OK]\r\n", evt.value.v);
-            if (evt.value.v == QUEUE_PUT_ISR_VALUE) {
-                isr_puts_counter++;
-            }
-            if (isr_puts_counter >= QUEUE_SIZE) {
-                break;
-            }
-        }
-    }
-
-    MBED_HOSTTEST_RESULT(result);
-    return 0;
-}
-
-#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TuxLeon/code/mbed-dev/#1f431a9e9227
--- a/mbed-rtos.lib	Wed Nov 11 07:53:46 2015 +0000
+++ b/mbed-rtos.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/mzta/code/mbed-rtos/#8ac966e2cd3f
+http://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- a/mbed-src.lib	Wed Nov 11 07:53:46 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed-src/#a11c0372f0ba
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pca9685.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/teams/BLUESINK/code/pca9685/#5c8802f876f8
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ppCANOpen.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ptpaterson/code/ppCANOpen/#22a337cdc0e3
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic.lib	Sun Feb 11 18:41:01 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f
--- a/test_env.cpp	Wed Nov 11 07:53:46 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,93 +0,0 @@
-#include "test_env.h"
-
-// Const strings used in test_end
-const char* TEST_ENV_START = "start";
-const char* TEST_ENV_SUCCESS = "success";
-const char* TEST_ENV_FAILURE = "failure";
-const char* TEST_ENV_MEASURE = "measure";
-const char* TEST_ENV_END = "end";
-
-
-static void led_blink(PinName led, float delay)
-{
-    if (led != NC) {
-        DigitalOut myled(led);
-        while (1) {
-            myled = !myled;
-            wait(delay);
-        }
-    }
-    while(1);
-}
-
-void notify_start()
-{
-    printf("{{%s}}" NL, TEST_ENV_START);
-}
-
-void notify_performance_coefficient(const char* measurement_name, const int value)
-{
-    printf("{{%s;%s;%d}}" RCNL, TEST_ENV_MEASURE, measurement_name, value);
-}
-
-void notify_performance_coefficient(const char* measurement_name, const unsigned int value)
-{
-    printf("{{%s;%s;%u}}" RCNL, TEST_ENV_MEASURE, measurement_name, value);
-}
-
-void notify_performance_coefficient(const char* measurement_name, const double value)
-{
-    printf("{{%s;%s;%f}}" RCNL, TEST_ENV_MEASURE, measurement_name, value);
-}
-
-void notify_completion(bool success)
-{
-    printf("{{%s}}" NL "{{%s}}" NL, success ? TEST_ENV_SUCCESS : TEST_ENV_FAILURE, TEST_ENV_END);
-    led_blink(LED1, success ? 1.0 : 0.1);
-}
-
-bool notify_completion_str(bool success, char* buffer)
-{
-    bool result = false;
-    if (buffer) {
-        sprintf(buffer, "{{%s}}" NL "{{%s}}" NL, success ? TEST_ENV_SUCCESS : TEST_ENV_FAILURE, TEST_ENV_END);
-        result = true;
-    }
-    return result;
-}
-
-// Host test auto-detection API
-void notify_host_test_name(const char *host_test) {
-    if (host_test) {
-        printf("{{host_test_name;%s}}" NL, host_test);
-    }
-}
-
-void notify_timeout(int timeout) {
-    printf("{{timeout;%d}}" NL, timeout);
-}
-
-void notify_test_id(const char *test_id) {
-    if (test_id) {
-        printf("{{test_id;%s}}" NL, test_id);
-    }
-}
-
-void notify_test_description(const char *description) {
-    if (description) {
-        printf("{{description;%s}}" NL, description);
-    }
-}
-
-
-// -DMBED_BUILD_TIMESTAMP=1406208182.13
-unsigned int testenv_randseed()
-{
-    unsigned int seed = 0;
-#ifdef MBED_BUILD_TIMESTAMP
-    long long_seed = static_cast<long>(MBED_BUILD_TIMESTAMP);
-    seed = long_seed & 0xFFFFFFFF;
-#endif /* MBED_BUILD_TIMESTAMP */
-    return seed;
-}
-
--- a/test_env.h	Wed Nov 11 07:53:46 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,75 +0,0 @@
-#ifndef TEST_ENV_H_
-#define TEST_ENV_H_
-
-#include <stdio.h>
-#include "mbed.h"
-
-#define NL "\n"
-#define RCNL "\r\n"
-
-// Const strings used in test_end
-extern const char* TEST_ENV_START;
-extern const char* TEST_ENV_SUCCESS;
-extern const char* TEST_ENV_FAILURE;
-extern const char* TEST_ENV_MEASURE;
-extern const char* TEST_ENV_END;
-
-// Test result related notification functions
-void notify_start();
-void notify_completion(bool success);
-bool notify_completion_str(bool success, char* buffer);
-void notify_performance_coefficient(const char* measurement_name, const int value);
-void notify_performance_coefficient(const char* measurement_name, const unsigned int value);
-void notify_performance_coefficient(const char* measurement_name, const double value);
-
-// Host test auto-detection API
-void notify_host_test_name(const char *host_test);
-void notify_timeout(int timeout);
-void notify_test_id(const char *test_id);
-void notify_test_description(const char *description);
-
-// Host test auto-detection API
-#define MBED_HOSTTEST_START(TESTID)      notify_test_id(TESTID); notify_start()
-#define MBED_HOSTTEST_SELECT(NAME)       notify_host_test_name(#NAME)
-#define MBED_HOSTTEST_TIMEOUT(SECONDS)   notify_timeout(SECONDS)
-#define MBED_HOSTTEST_DESCRIPTION(DESC)  notify_test_description(#DESC)
-#define MBED_HOSTTEST_RESULT(RESULT)     notify_completion(RESULT)
-
-/**
-    Test auto-detection preamble example:
-    main() {
-        MBED_HOSTTEST_TIMEOUT(10);
-        MBED_HOSTTEST_SELECT( host_test );
-        MBED_HOSTTEST_DESCRIPTION(Hello World);
-        MBED_HOSTTEST_START("MBED_10");
-        // Proper 'host_test.py' should take over supervising of this test
-
-        // Test code
-        bool result = ...;
-
-        MBED_HOSTTEST_RESULT(result);
-    }
-*/
-
-
-// Test functionality useful during testing
-unsigned int testenv_randseed();
-
-// Macros, unit test like to provide basic comparisons
-#define TESTENV_STRCMP(GIVEN,EXPECTED) (strcmp(GIVEN,EXPECTED) == 0)
-
-// macros passed via test suite
-#ifndef TEST_SUITE_TARGET_NAME
-#define TEST_SUITE_TARGET_NAME "Unknown"
-#endif
-
-#ifndef TEST_SUITE_TEST_ID
-#define TEST_SUITE_TEST_ID "Unknown"
-#endif
-
-#ifndef TEST_SUITE_UUID
-#define TEST_SUITE_UUID "Unknown"
-#endif
-
-#endif
-