zoukankan      html  css  js  c++  java
  • 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;
    
                }
    
    
    
            }
    
    
        }
    
        
    }
    
    
    
    透过泪水看到希望
  • 相关阅读:
    WCF – System.ServiceModel.FaultException
    SSB – Connection handshake failed
    JQuery语法总结和注意事项
    删除指定目录及其子目录和文件
    Windows 7中还不能通过“HyperV管理器”连接HyperV Server 2008 R2服务器解决
    tomcat虚拟目录映射网络共享目录的问题
    Creating a Virtual Directory with a UNC Path (IIS 6.0)
    百度地图api根据地区名称反查其经纬度的实例
    远程管理server core上的hyperv
    ios5中使用Reachability的问题
  • 原文地址:https://www.cnblogs.com/ronnielee/p/10030963.html
Copyright © 2011-2022 走看看