cao yang
kjkjkjk
Revision 2:a6b8baa4e4b7, committed 2019-05-12
- Comitter:
- mihaidd
- Date:
- Sun May 12 13:39:08 2019 +0000
- Parent:
- 1:0c82ff54d168
- Commit message:
- kjkjk
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 0c82ff54d168 -r a6b8baa4e4b7 main.cpp
--- a/main.cpp Wed May 08 04:02:50 2019 +0000
+++ b/main.cpp Sun May 12 13:39:08 2019 +0000
@@ -12,7 +12,7 @@
#define IDENTIFICATIONMODEL_ID 0x0000
-VL6180 TOF_sensor(I2C_SDA, I2C_SCL);
+VL6180 TOF_sensor(I2C_SDA, I2C_SCL);//距离的串口定义
DigitalOut myled0(LED2);
#define BACK_LIGHT_ON(INTERFACE) INTERFACE->write_bit(1,BL_BIT)
@@ -20,27 +20,36 @@
MCP23017 *par_port;
WattBob_TextLCD *lcd;
-Serial STM2Android(PC_4,PC_5,9600);
-I2C i2c(PC_1, PC_0); //pins for I2C communication (SDA, SCL)
+Serial STM2Android(PC_4,PC_5,9600);//上位机串口定义
+I2C i2c1(PC_1, PC_0); //pins for I2C communication (SDA, SCL) 人颜色的串口定义
+I2C i2c2(PB_14, PB_13); //机器人颜色的串口定义
-DigitalOut d(D3); //dianjide diyiwei
-//DigitalOut j(D4); //dianjide dierwei
-
-DigitalOut a(PC_10);//hongwaide yi
-DigitalOut b(PC_12);//hongwaide er
+DigitalOut d(PB_3); //电机的第一位
+DigitalOut j(PA_9); //电机的第二位
+DigitalOut k(PC_7); //电机的第三位
+DigitalOut a(PC_10);//红外的一
+DigitalOut b(PC_12);//红外的二
int sensor_addr = 41 << 1;
-DigitalOut green(LED1);
char flag_Android[]="";
char flag_Android_1[]="begin";
-
+char flag_Android_2[]="distance";
+char Android_0[]="";
+char Android_1='l';
+char flag_Android_3[]="repair";
int main()
{
- int goal_1=0;
- int goal_2=0;
- d=1;
- uint8_t dist;
+ //int goal_1=0;//人类得分
+// int goal_2=0;//机器人得分
+// d=1;
+// j=1;
+// k=1;
+// int kehu=0;//客户类型
+ d=1;
+ j=1;
+ k=1;
+ uint8_t dist;//距离的值
//julichuanganqi
par_port = new MCP23017(I2C_SDA, I2C_SCL,0x0400);
par_port->config(0x0F00, 0x0F00, 0x0F00); // configure MCP23017 chip on WattBob
@@ -50,143 +59,204 @@
while(1)
{
- STM2Android.scanf("%s",&flag_Android);
- if(strcmp(flag_Android,flag_Android_1)==0)
- {
-
+ int goal_1=0;//人类得分
+ int goal_2=0;//机器人得分
+
+ int kehu=0;//客户类型
+
dist = TOF_sensor.getDistance();
lcd->cls();
lcd->locate(0,0);
- green = 1; // off
- // Connect to the Color sensor and verify whether we connected to the correct sensor.
- i2c.frequency(100000);//修改为100000，否则报错
+ i2c1.frequency(100000);//修改为100000，否则报错
+ i2c2.frequency(100000);
+
char id_regval[1] = {146};
char data[1] = {0};
- i2c.write(sensor_addr,id_regval,1, true);
- i2c.read(sensor_addr,data,1,false);
- if (data[0]==68)
- {
- green = 0;
- wait (2);
- green = 1;
- }
- else
- {
- green = 1;
- }
- // Initialize color sensor
+ i2c1.write(sensor_addr,id_regval,1, true);
+ i2c1.read(sensor_addr,data,1,false);
+ i2c2.write(sensor_addr,id_regval,1, true);
+ i2c2.read(sensor_addr,data,1,false);
+
char timing_register[2] = {129,0};
- i2c.write(sensor_addr,timing_register,2,false);
+ i2c1.write(sensor_addr,timing_register,2,false);
+ i2c2.write(sensor_addr,timing_register,2,false);
+
char control_register[2] = {143,0};
- i2c.write(sensor_addr,control_register,2,false);
+ i2c1.write(sensor_addr,control_register,2,false);
+ i2c2.write(sensor_addr,control_register,2,false);
+
char enable_register[2] = {128,3};
- i2c.write(sensor_addr,enable_register,2,false);
+ i2c1.write(sensor_addr,enable_register,2,false);
+ i2c2.write(sensor_addr,enable_register,2,false);
// Read data from color sensor (Clear/Red/Green/Blue)
+
char clear_reg[1] = {148};
- char clear_data[2] = {0,0};
- i2c.write(sensor_addr,clear_reg,1, true);
- i2c.read(sensor_addr,clear_data,2, false);
- int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
+ char clear_data1[2] = {0,0};
+ char clear_data2[2] = {0,0};
+
+ i2c1.write(sensor_addr,clear_reg,1, true);
+ i2c1.read(sensor_addr,clear_data1,2, false);
+ i2c2.write(sensor_addr,clear_reg,1, true);
+ i2c2.read(sensor_addr,clear_data2,2, false);
+
+ int clear_value1 = ((int)clear_data1[1] << 8) | clear_data1[0];
+ int clear_value2 = ((int)clear_data2[1] << 8) | clear_data2[0];
+
char red_reg[1] = {150};
- char red_data[2] = {0,0};
- i2c.write(sensor_addr,red_reg,1, true);
- i2c.read(sensor_addr,red_data,2, false);
- int red_value = ((int)red_data[1] << 8) | red_data[0];
+ char red_data1[2] = {0,0};
+ char red_data2[2] = {0,0};
+
+ i2c1.write(sensor_addr,red_reg,1, true);
+ i2c1.read(sensor_addr,red_data1,2, false);
+ i2c2.write(sensor_addr,red_reg,1, true);
+ i2c2.read(sensor_addr,red_data2,2, false);
+
+ int red_value1 = ((int)red_data1[1] << 8) | red_data1[0];
+ int red_value2 = ((int)red_data2[1] << 8) | red_data2[0];
+
char green_reg[1] = {152};
- char green_data[2] = {0,0};
- i2c.write(sensor_addr,green_reg,1, true);
- i2c.read(sensor_addr,green_data,2, false);
- int green_value = ((int)green_data[1] << 8) | green_data[0];
+ char green_data1[2] = {0,0};
+ char green_data2[2] = {0,0};
+
+ i2c1.write(sensor_addr,green_reg,1, true);
+ i2c1.read(sensor_addr,green_data1,2, false);
+ i2c2.write(sensor_addr,green_reg,1, true);
+ i2c2.read(sensor_addr,green_data2,2, false);
+
+ int green_value1 = ((int)green_data1[1] << 8) | green_data1[0];
+ int green_value2 = ((int)green_data2[1] << 8) | green_data2[0];
+
char blue_reg[1] = {154};
- char blue_data[2] = {0,0};
- i2c.write(sensor_addr,blue_reg,1, true);
- i2c.read(sensor_addr,blue_data,2, false);
- int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
- // print sensor readings
- wait(20);
- if (red_value>green_value&&red_value>blue_value) //shifoukeyiyizhichuanzhi???????????????
+ char blue_data1[2] = {0,0};
+ char blue_data2[2] = {0,0};
+
+ i2c1.write(sensor_addr,blue_reg,1, true);
+ i2c1.read(sensor_addr,blue_data1,2, false);
+ i2c2.write(sensor_addr,blue_reg,1, true);
+ i2c2.read(sensor_addr,blue_data2,2, false);
+
+ int blue_value1 = ((int)blue_data1[1] << 8) | blue_data1[0];
+ int blue_value2 = ((int)blue_data2[1] << 8) | blue_data2[0];
+ // print sensor readings
+
+ STM2Android.scanf("%s",&Android_0);//要给上位机传输dist以及用户识别
+ if(strcmp(Android_0,flag_Android_2)==0)
+ {
+ // if (a==0&&b==0) kehu=0;//用户识别 小孩
+ // else if (a==0&&b==1) kehu=1;//青年
+ //else if (a==1&&b==0) kehu=2;//老人
+ //STM2Android.printf("x%d\r\n",kehu);
+ wait(3);
+ // if(dist<=180)
+ STM2Android.printf("e");//距离合适
+
+ /* else if(strcmp(Android_0,flag_Android_3)==0)//维护模式
+ {
+ STM2Android.scanf("%c",&Android_1);
+ if(Android_1=='c')STM2Android.printf("dClear1(%d),Red1(%d),Green1(%d),Blue1(%d)\r\n", clear_value1, red_value1, green_value1, blue_value1);
+ else if(Android_1=='d')STM2Android.printf("dd=(%d)\r\n",dist);
+ else if(Android_1=='e')
+ {
+ d=0;j=0;k=0;//电机转动
+ wait(10);
+ d=1;j=1;k=1;//电机停止转动
+ }
+ }
+ */
+ wait(2);
+ STM2Android.scanf("%s",&flag_Android);
+ if(strcmp(flag_Android,flag_Android_1)==0)
+ {
+ wait(1);
+ if ((red_value1>green_value1)&&(red_value1>blue_value1)) //shifoukeyiyizhichuanzhi???????????????
{
- STM2Android.printf("a2");//red+2
+ STM2Android.printf("a3");//red+3
+ goal_1=3;
+ }
+ if ((green_value1>red_value1)&&(green_value1>blue_value1)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("a2");//green+2
goal_1=2;
}
- else if (green_value>red_value&&green_value>blue_value) //shifoukeyiyizhichuanzhi???????????????
+ if ((blue_value1>green_value1)&&(blue_value1>red_value1)) //shifoukeyiyizhichuanzhi???????????????
{
- STM2Android.printf("a1");//green+1
+ STM2Android.printf("a1");//blue+1
goal_1=1;
}
- else if (red_value>green_value&&red_value>blue_value) //shifoukeyiyizhichuanzhi???????????????
- {
- STM2Android.printf("a0");//blue+0
- goal_=0;
- }
+ wait(1);
+ d=0;j=0;k=0;//电机转动
wait(5);
-
-
-
+ d=1;j=1;k=1;//电机停止转动
+ wait(1);
-
- d=0;//dianjizhuandong
- wait(10);
- d=1;//dianjitingzhizhuandong
- wait(2);
-
-
-
- if (red_value>green_value&&red_value>blue_value) //shifoukeyiyizhichuanzhi???????????????
+ if ((red_value2>green_value2)&&(red_value2>blue_value2)) //shifoukeyiyizhichuanzhi???????????????
{
- STM2Android.printf("b2");//red+2
+ STM2Android.printf("b3");//red+3
+ goal_2=3;
+ }
+ if ((green_value2>red_value2)&&(green_value2>blue_value2)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("b2");//green+2
goal_2=2;
}
- else if (green_value>red_value&&green_value>blue_value) //shifoukeyiyizhichuanzhi???????????????
- {
- STM2Android.printf("b1");//green+1
- goal_2=1;
- }
- else if (red_value>green_value&&red_value>blue_value) //shifoukeyiyizhichuanzhi???????????????
+ if ((blue_value2>green_value2)&&(blue_value2>red_value2)) //shifoukeyiyizhichuanzhi???????????????
{
- STM2Android.printf("b0");//blue+0
- goal_2=0;
- }
- if(goal_1>goal_2)
+ STM2Android.printf("b1");//blue+1
+ goal_2=1;
+ }
+ wait(1);
- // if (a==0&&b==0) kehu=0;//yonghushibie
- // else if (a==0&&b==1) kehu=1;
- // else if (a==1&&b==0) kehu=2;
- // else if (a==1&&b==1) kehu=3;
-
+ if(goal_1>goal_2)
+ {
+ STM2Android.printf("c1");//ren win
+ if(kehu==0)
+ {
+ d=0;
+ j=0;
+ k=1;
+ wait(5);
+ d=1;
+ j=1;
+ k=1;
+ }
+ else if(kehu==1)
+ {
+ d=0;
+ j=1;
+ k=0;
+ }
+ else if(kehu==2)
+ {
+ d=1;
+ j=0;
+ k=0;
+ }
+ }
+
+ //
+
+ if(goal_1<=goal_2)
+ STM2Android.printf("c0");//robot win
+
+ // STM2Android.printf("aClear(%d),Red(%d),Green(%d),Blue(%d),d=(%d)\r\n", clear_value, red_value, green_value, blue_value, dist);
+ //STM2Android.printf("ad=(%d)\r\n",dist);
+ }//小if语句判断结束
+ }//大if语句判断结束
+
+
+
+ // else if(strcmp(Android_0,flag_Android_3)==0)//维护模式
+// {
+// STM2Android.scanf("%c",&Android_1);
+// if(Android_1=='c')STM2Android.printf("dClear1(%d),Red1(%d),Green1(%d),Blue1(%d)\r\n", clear_value1, red_value1, green_value1, blue_value1);
+// else if(Android_1=='d')STM2Android.printf("dd=(%d)\r\n",dist);
+// else if(Android_1=='e')
+// {
+// d=0;j=0;k=0;//电机转动
+// wait(10);
+// d=1;j=1;k=1;//电机停止转动
+// }
+// }
- // STM2Android.printf("aClear(%d),Red(%d),Green(%d),Blue(%d),d=(%d)\r\n", clear_value, red_value, green_value, blue_value, dist);
- //STM2Android.printf("ad=(%d)\r\n",dist);
- STM2Android.printf("a1");
-
- d=0;//dianjizhuandong
- wait(10);
- d=1;
-
- // d=1;
- //wait(2);
- }
-
-
-
-
-
- // else{
- //lingyigechuanganqi
- // z=0;
- // y=0;s=1;
- // wait(2);
- // y=1;s=0;
- // wait(2);
-
- // dist = TOF_sensor.getDistance();
- // STM2Android.printf("d=%d", dist);
- //wait(0.5);
- // lcd->cls();
- //lcd->locate(0,0);
-
- //wait(5);
-
- // }
- }
+ }//while循环结束
}
\ No newline at end of file