success

#include "mbed.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"
#include "TCS3472_I2C.h"
#include "stdint.h"
#include "VL6180.h"
#include "rtos.h"

DigitalIn Ain(p21);
DigitalIn Bin(p22);
DigitalIn Cin(p23);
DigitalIn Din(p24);

DigitalOut Servo1a(p5);
DigitalOut Servo1b(p6);


DigitalOut Servo2a(p7);
DigitalOut Servo2b(p8);
DigitalOut Servo3a(p9);
DigitalOut Servo3b(p10);

DigitalOut Servo4(p11);

Serial pc(USBTX, USBRX);

Thread thread1;
Thread thread5;
Thread thread2;
Thread thread3;
Thread thread4;

void servo1_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x001);

        Servo1a = 0;
        Servo1b = 1;
        wait(0.2);
        Servo1a = 1;
    }
}

void servo5_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x010);

        Servo1a = 1;
        Servo1b = 0;
        wait(0.2);
        Servo1a = 0;
        
    }
}


void servo2_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x011);
        while(1)
        {
            Servo2a = 0;
            Servo2b = 0;
            wait(1);
            Servo2a = 1;
            Servo2b = 1;
            wait(1);
        }
    }
}

void servo3_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x100);
        while(1)
        {
            Servo3a = 0;
            Servo3b = 0;
            wait(1);
            Servo3a = 1;
            Servo3b = 1;
            wait(1);
        }
    }
}

void servo4_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x101);
        Servo4 = 0;
        wait(0.5);
        Servo4 = 1;
    }
}


int main() 
{
    


    thread1.start(servo1_thread);
    thread5.start(servo5_thread);
    thread2.start(servo2_thread);
    thread3.start(servo3_thread);
    thread4.start(servo4_thread);



    
    char c = 'q';
    while(1)
    {
        c = pc.getc();
        if(c == '1')
            thread1.signal_set(0x001);
        if(c == '5')
            thread5.signal_set(0x010);
        if(c == '2')
            thread2.signal_set(0x011);
        if(c == '3')
            thread3.signal_set(0x100);
        if(c == '4')
            thread4.signal_set(0x101);
        if(c == 'q')
        {
            thread1.start(servo1_thread);
            thread5.start(servo5_thread);
            thread2.start(servo2_thread);
            thread3.start(servo3_thread);
            thread4.start(servo4_thread);
        }
        if(c == 'w')
        {
            thread1.terminate( );
            thread5.terminate( );
            thread2.terminate( );
            thread3.terminate( );
            thread4.terminate( );
        }
    }

    
}

#include "mbed.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"
#include "TCS3472_I2C.h"
#include "stdint.h"
#include "VL6180.h"
#include "rtos.h"

DigitalIn Ain(p21);
DigitalIn Bin(p22);
DigitalIn Cin(p23);
DigitalIn Din(p24);

DigitalOut Servo1a(p5);
DigitalOut Servo1b(p6);


DigitalOut Servo2a(p7);
DigitalOut Servo2b(p8);
DigitalOut Servo3a(p9);
DigitalOut Servo3b(p10);

DigitalOut Servo4(p11);

Serial pc(USBTX, USBRX);

Thread thread1;
Thread thread5;
Thread thread2;
Thread thread3;
Thread thread4;

void servo1_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x001);

        Servo1a = 0;
        Servo1b = 1;
        wait(0.2);
        Servo1a = 1;
    }
}

void servo5_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x010);

        Servo1a = 1;
        Servo1b = 0;
        wait(0.2);
        Servo1a = 0;
        
    }
}


void servo2_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x011);
        while(1)
        {
            Servo2a = 0;
            Servo2b = 0;
            wait(1);
            Servo2a = 1;
            Servo2b = 1;
            wait(1);
        }
    }
}

void servo3_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x100);
        while(1)
        {
            Servo3a = 0;
            Servo3b = 0;
            wait(1);
            Servo3a = 1;
            Servo3b = 1;
            wait(1);
        }
    }
}

void servo4_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x101);
        Servo4 = 0;
        wait(1);
        Servo4 = 1;
    }
}


int main() 
{
    


    thread1.start(servo1_thread);
    thread5.start(servo5_thread);
    thread2.start(servo2_thread);
    thread3.start(servo3_thread);
    thread4.start(servo4_thread);



    
    char c = 'q';
    while(1)
    {
        c = pc.getc();
        if(c == '1')
            thread1.signal_set(0x001);
        if(c == '5')
            thread5.signal_set(0x010);
        if(c == '2')
            thread2.signal_set(0x011);
        if(c == '3')
            thread3.signal_set(0x100);
        if(c == '4')
            thread4.signal_set(0x101);
        if(c == 'q')
            break;
        if(c == 'w')
        {
            thread1.terminate( );
            thread5.terminate( );
            thread2.terminate( );
            thread3.terminate( );
            thread4.terminate( );
        }
    }

    
}