- #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 Servo5(p16);
- DigitalIn Ein(p20);
- Serial pc(USBTX, USBRX);
- Thread thread1;
- Thread thread2;
- Thread thread3;
- Thread thread4;
- Thread thread5;
- Thread thread6;
- Thread thread7;
- Thread thread8;
- void servo1_thread()
- {
- while (true)
- {
- Thread::signal_wait(0x001);
- Servo1a = 0;
- Servo1b = 1;
- wait(0.2);
- Servo1a = 1;
- }
- }
- void servo2_thread()
- {
- while (true)
- {
- Thread::signal_wait(0x010);
- Servo1a = 1;
- Servo1b = 0;
- wait(0.2);
- 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);
- Servo5 = 0;
- wait(0.5);
- Servo5 = 1;
- wait(0.5);
- Servo5 = 0;
- Servo4 = 0;
- wait(0.5);
- Servo4 = 1;
- wait(0.5);
- Servo4 = 0;
- }
- }
- bool p = false;
- void servo8_thread()
- {
- while (true)
- {
- if(Ein == 1)
- {
- pc.printf("s");
- thread1.terminate( );
- thread2.terminate( );
- thread3.terminate( );
- thread4.terminate( );
- thread5.terminate( );
- thread6.terminate( );
- // thread7.signal_set(0x101);
- // wait(6);
- // thread7.terminate( );
- p = true;
- thread8.terminate( );
- }
- }
- }
- 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 = '/';
- while(1)
- {
- //Distance Sensor
- while(1)
- {
- c = pc.getc();
- if(c == 's')
- {
- dist = TOF_sensor.getDistance();
- dist /= 10;
- //wait(3);
- //pc.printf("%d", dist);
- if(dist>=11)
- pc.printf("l");
- if(dist<11)
- pc.printf("s");
- }
- 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);
- }
- //if sensor detected
- //Undo
- if(c == 's')
- {
- //thread1.terminate( );
- //thread2.terminate( );
- thread3.terminate( );
- thread4.terminate( );
- thread5.terminate( );
- thread6.terminate( );
- //thread7.signal_set(0x101);
- //wait(6);
- //thread7.terminate( );
- }
- if(c == 'b')
- {
- thread1.terminate( );
- thread2.terminate( );
- thread3.terminate( );
- thread4.terminate( );
- thread5.terminate( );
- thread6.terminate( );
- n = false;
- }
- }
- //Color Sensor
- while(1)
- {
- 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')
- {
- break;
- }
- }
- //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);
- thread7.start(servo7_thread);
- thread8.start(servo8_thread);
- c = pc.getc();
- 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);
- }
- if(p == true)
- {
- thread1.terminate( );
- thread2.terminate( );
- thread3.terminate( );
- thread4.terminate( );
- thread5.terminate( );
- thread6.terminate( );
- //n = false;
- break;
- }
- //if sensor detected
- //C_sharp sent 'f' if time is up
- if(c == 'f')
- {
- //Rotate the servo to reset the ball
- thread1.terminate( );
- thread2.terminate( );
- thread3.terminate( );
- thread4.terminate( );
- thread5.terminate( );
- thread6.terminate( );
- thread7.signal_set(0x101);
- wait(6);
- thread7.terminate( );
- break;
- }
- }
- }
- }
- new
来源: http://www.bubuko.com/infodetail-2860624.html