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;
}
}
}
}
透过泪水看到希望
