1128a

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

#define     BACK_LIGHT_ON(INTERFACE)    INTERFACE->write_bit(1,BL_BIT)
#define     BACK_LIGHT_OFF(INTERFACE)   INTERFACE->write_bit(0,BL_BIT)
#define IDENTIFICATIONMODEL_ID 0x0000
MCP23017            *par_port;
WattBob_TextLCD     *lcd;

TCS3472_I2C rgb_sensor(p9, p10);//9:data
VL6180  TOF_sensor(p28, p27);//28:data
DigitalIn Ain(p21);
DigitalIn Bin(p22);
DigitalIn Cin(p23);
DigitalIn Din(p24);

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

DigitalOut Servo2a(p11);
DigitalOut Servo2b(p12);
DigitalOut Servo3a(p13);
DigitalOut Servo3b(p14);

DigitalOut Servo4(p15);

DigitalOut Magnet(p17);

DigitalIn Ein(p18);

DigitalOut Fout(p19);

DigitalIn Gin(p20);


Serial pc(USBTX, USBRX);

Thread thread1;
Thread thread2;
Thread thread3;
Thread thread4;
Thread thread5;
Thread thread6;
Thread thread7;
Thread thread8;
Thread thread9;
Thread thread10;


char d = '/';
void servo1_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x001);

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

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

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


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

    }
}

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


void servo5_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x100);
        while(1)
        {
            Servo2a = 0;
            Servo2b = 0;
            wait(2);
            Servo2a = 1;
            Servo2b = 1;
            wait(2);
        }

    }
}

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


void servo7_thread() 
{
    while (true) 
    {
        
        Thread::signal_wait(0x101);

        Servo4 = 0;
        wait(0.5);


        Magnet = 0;
        wait(0.5);
        Magnet = 1;
        wait(0.5);
        Magnet = 0;
        
        wait(1);
        Servo4 = 1;
        wait(1);
        Servo4 = 0;

        

    }
}


void read_thread()
{ 
    d = pc.getc();

}

void servo8_thread() 
{
    thread9.terminate( );
    if(Ein == 1)
    {
        pc.printf("w");
        wait(1);
        //myled = !myled;
    }
    thread9.start(read_thread);
}


void servo9_thread() 
{
    thread9.terminate( );
    if(Gin == 0)
    {
        pc.printf("f");
        wait(1);
        //myled = !myled;
    }
    thread9.start(read_thread);
}

int main() 
{

    
    uint8_t dist;
    
    TOF_sensor.VL6180_Init();
    par_port = new MCP23017(p9, p10, 0x40);
    par_port->config(0x0F00, 0x0F00, 0x0F00); 

    BACK_LIGHT_ON(par_port);

    int rgb_readings[4];
    
    rgb_sensor.enablePowerAndRGBC();
    rgb_sensor.setIntegrationTime(100);
      
    char c;
    c = '/';
    Fout = 1;
    while(1)
    {  
        // while(1)
        // {
        //     if(Gin == 0)
        //     {
        //         pc.printf("f");
        //         wait(1);
        //         //myled = !myled;
        //     }
        // }

        //Distance Sensor
        while(1)
        {
            c = pc.getc();


            // for(int i = 0 ; i < 5 ;i++)
            //     {
            //         Fout = 1;
            //         wait(0.3);
            //         Fout = 0;
            //         wait(0.3);

            //     }

            if(c == 'x')
            {
                thread1.start(servo1_thread);
                thread2.start(servo2_thread);
                for(int i = 0 ; i < 6 ;i++)
                {
                    thread1.signal_set(0x001);
                    wait(0.5);
                }
                for(int i = 0 ; i < 4 ;i++)
                {
                    thread2.signal_set(0x010);
                    wait(0.5);
                }
                thread1.terminate( );
                thread2.terminate( );
            }
            
            if(c == 's')
            {
                dist = TOF_sensor.getDistance();
                dist /= 9;
                //pc.printf("%d", dist);
                if(dist<11)
                    pc.printf("s");

                else if(dist>=11&&dist<=20)
                    pc.printf("m");
                else
                //if(dist<11)
                    pc.printf("l");
            }
            if(c == 'o')
            {
                break;
            }
        }




        //Photoelectric Sensor
        //Special Ain
        bool n = false;
        while(1)
        {
            c = pc.getc();
            if(c == '1')
            {
                
            if(Ain==1&&Bin==0&&Cin==0&&Din==0)
                pc.printf("1"), n = true;
            if(Ain==1&&Bin==0&&Cin==0&&Din==1)
                pc.printf("2"), n = false;
            if(Ain==1&&Bin==0&&Cin==1&&Din==0)
                pc.printf("3"), n = false;
            if(Ain==1&&Bin==0&&Cin==1&&Din==1)
                pc.printf("4"), n = false;
            if(Ain==1&&Bin==1&&Cin==0&&Din==0)
                pc.printf("5"), n = false;
            if(Ain==1&&Bin==1&&Cin==0&&Din==1)
                pc.printf("6"), n = false;
            if(Ain==1&&Bin==1&&Cin==1&&Din==0)
                pc.printf("7"), n = false;
            if(Ain==1&&Bin==1&&Cin==1&&Din==1)
                pc.printf("8"), n = false;
            if(Ain==0&&Bin==0&&Cin==0&&Din==0)
                pc.printf("9"), n = false;
            if(Ain==0&&Bin==0&&Cin==0&&Din==1)
                pc.printf("10"), n = false;
            if(Ain==0&&Bin==0&&Cin==1&&Din==0)
                pc.printf("11"), n = false;
            if(Ain==0&&Bin==0&&Cin==1&&Din==1)
                pc.printf("12"), n = false;
            if(Ain==0&&Bin==1&&Cin==0&&Din==0)
                pc.printf("13"), n = false;
            if(Ain==0&&Bin==1&&Cin==0&&Din==1)
                pc.printf("14"), n = false;
            if(Ain==0&&Bin==1&&Cin==1&&Din==0)
                pc.printf("15"), n = false;
            if(Ain==0&&Bin==1&&Cin==1&&Din==1)
                pc.printf("16");
            }
            if(c == '2')
                break;
            
        }

        while(n == true)
        {
            thread1.start(servo1_thread);
            thread2.start(servo2_thread);

            thread3.start(servo3_thread);
            thread4.start(servo4_thread);
            thread5.start(servo5_thread);
            thread6.start(servo6_thread);

            thread7.start(servo7_thread);
            c = pc.getc();


            if(c == 'd')
            {
                dist = TOF_sensor.getDistance();
                dist /= 10;
                //wait(3);
                pc.printf("%d", dist);
            }

            if(c == 'c')
            {      
                rgb_sensor.getAllColors(rgb_readings);

                if(rgb_readings[1]>rgb_readings[2]&&rgb_readings[1]>rgb_readings[3])
                {
                    pc.printf("r");
                }

                          
                if(rgb_readings[2]>rgb_readings[1]&&rgb_readings[2]>rgb_readings[3])
                {
                    pc.printf("g");
                }
                    
                if(rgb_readings[3]>rgb_readings[1]&&rgb_readings[3]>rgb_readings[2])
                {
                    pc.printf("b");
                }
            }


            if(c == 'e')
            {
                thread5.start(servo5_thread);
                thread6.start(servo6_thread);
                thread5.signal_set(0x100);
                thread6.signal_set(0x100);
            }

            if(c == 'h')
            {
                thread3.start(servo3_thread);
                thread4.start(servo4_thread);
                thread3.signal_set(0x011);
                thread4.signal_set(0x011);
            }

            if(c == 'l')
            {
                
                thread1.signal_set(0x001);

            }
                
            if(c == 'r')
            {

                thread2.signal_set(0x010);
            
            }
                
            
            //Stop
            if(c == 's')
            {
                thread3.terminate( );
                thread4.terminate( );
                thread5.terminate( );
                thread6.terminate( );
            }

            //Break
            if(c == 'b')
            {
                thread1.terminate( );
                thread2.terminate( );
                thread3.terminate( );
                thread4.terminate( );
                thread5.terminate( );
                thread6.terminate( );
                thread7.terminate( );
                n = false;
            }


               
        }

        n = true;
        //Color Sensor
        while(n)
        {
            c = pc.getc();
            if(c == 'a')
            {      
                rgb_sensor.getAllColors(rgb_readings);

                if(rgb_readings[1]>rgb_readings[2]&&rgb_readings[1]>rgb_readings[3])
                {
                    pc.printf("r");
                    continue;
                }

                          
                if(rgb_readings[2]>rgb_readings[1]&&rgb_readings[2]>rgb_readings[3])
                {
                    pc.printf("g");
                    continue;
                }
                    
                if(rgb_readings[3]>rgb_readings[1]&&rgb_readings[3]>rgb_readings[2])
                {
                    pc.printf("b");
                    continue;
                }
            }

            if(c == 'b')
            {

                thread7.start(servo7_thread);
                thread7.signal_set(0x101);
                wait(4);
                thread7.terminate( );
                d = '/';

                n = false;
            }

            if(c == 'c')
            {

                n = false;
            }    
        }

        //Servo
        while(1)
        {
            thread1.start(servo1_thread);
            thread2.start(servo2_thread);
            thread3.start(servo3_thread);
            thread4.start(servo4_thread);
            thread5.start(servo5_thread);
            thread6.start(servo6_thread);

            

            thread8.start(servo8_thread);
            thread10.start(servo9_thread);
            thread9.start(read_thread);


            

            if(d == 'x')
            {
                thread7.start(servo7_thread);
                thread7.signal_set(0x101);
                wait(4);
                thread7.terminate( );
                d = '/';
            }

            if(d == 'e')
            {
                thread5.start(servo5_thread);
                thread6.start(servo6_thread);
                thread5.signal_set(0x100);
                thread6.signal_set(0x100);
                d = '/';
            }

            if(d == 'h')
            {
                thread3.start(servo3_thread);
                thread4.start(servo4_thread);
                thread3.signal_set(0x011);
                thread4.signal_set(0x011);
                d = '/';
            }

            if(d == 'l')
            {
                
                thread1.signal_set(0x001);
                d = '/';

            }
                
            if(d == 'r')
            {

                thread2.signal_set(0x010);
                d = '/';
            
            }
            
            // if(d == ',')
            // {
            //     //Rotate the servo to reset the ball

            //     for(int i = 0 ; i < 10 ;i++)
            //     {
            //         Fout = 1;
            //         wait(0.1);
            //         Fout = 0;
            //         wait(0.9);

            //     }
            //     continue;



                
            //     //break;

            // }

            //C_sharp sent 'f' if time is up
            if(d == '.')
            {
                //Rotate the servo to reset the ball

                thread3.terminate( );
                thread4.terminate( );
                thread5.terminate( );
                thread6.terminate( );

                thread1.terminate( );
                thread2.terminate( );


                
                for(int i = 0 ; i < 20 ;i++)
                {
                    Fout = 1;
                    wait(0.1);
                    Fout = 0;
                    wait(0.1);

                }



                
                break;

            }

            if(d == '=')
            {
                //Rotate the servo to reset the ball

                thread3.terminate( );
                thread4.terminate( );
                thread5.terminate( );
                thread6.terminate( );

                thread1.terminate( );
                thread2.terminate( );


                
                for(int i = 0 ; i < 7 ;i++)
                {
                    Fout = 1;
                    wait(0.3);
                    Fout = 0;
                    wait(0.3);

                }

                break;

            }


            if(d == ';')
            {
                //Rotate the servo to reset the ball

                for(int i = 0 ; i < 6 ;i++)
                {
                    thread1.signal_set(0x001);
                    wait(0.5);
                }
                for(int i = 0 ; i < 6 ;i++)
                {
                    thread2.signal_set(0x010);
                    wait(0.5);
                }
                for(int i = 0 ; i < 6 ;i++)
                {
                    thread1.signal_set(0x001);
                    wait(0.5);
                }
                for(int i = 0 ; i < 6 ;i++)
                {
                    thread2.signal_set(0x010);
                    wait(0.5);
                }
                for(int i = 0 ; i < 3 ;i++)
                {
                    thread1.signal_set(0x001);
                    wait(0.5);
                }


                thread3.terminate( );
                thread4.terminate( );
                thread5.terminate( );
                thread6.terminate( );

                thread1.terminate( );
                thread2.terminate( );


                
                for(int i = 0 ; i < 20 ;i++)
                {
                    Fout = 1;
                    wait(0.1);
                    Fout = 0;
                    wait(0.1);

                }



                
                break;

            }



        }


    }

    
}