cao yang
yyyy
Diff: main.cpp
- Revision:
- 0:ae94058fb59a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon May 13 09:07:38 2019 +0000
@@ -0,0 +1,264 @@
+#include "mbed.h"
+#include "stdint.h"
+#include <string>
+#include "MCP23017.h"
+#include "WattBob_TextLCD.h"
+#include "VL6180.h"
+
+//Hyperterminal configuration
+//9600 bauds, 8-bit data, no parity
+
+//VL6180X defines
+
+#define IDENTIFICATIONMODEL_ID 0x0000
+
+VL6180 TOF_sensor(I2C_SDA, I2C_SCL);//距离的串口定义
+DigitalOut myled0(LED2);
+
+#define BACK_LIGHT_ON(INTERFACE) INTERFACE->write_bit(1,BL_BIT)
+#define BACK_LIGHT_OFF(INTERFACE) INTERFACE->write_bit(0,BL_BIT)
+
+MCP23017 *par_port;
+WattBob_TextLCD *lcd;
+Serial STM2Android(PC_4,PC_5,115200);//上位机串口定义
+I2C i2c1(PC_1, PC_0); //pins for I2C communication (SDA, SCL) 人颜色的串口定义
+I2C i2c2(PB_14, PB_13); //机器人颜色的串口定义
+
+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;
+
+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;
+ j=1;
+ k=1;
+ int kehu=0;//客户类型
+
+ uint8_t dist;//距离的值
+ //julichuanganqi
+ par_port = new MCP23017(I2C_SDA, I2C_SCL,0x0400);
+ par_port->config(0x0F00, 0x0F00, 0x0F00); // configure MCP23017 chip on WattBob
+ lcd = new WattBob_TextLCD(par_port);
+
+
+
+ while(1)
+ {
+
+ dist = TOF_sensor.getDistance();
+ lcd->cls();
+ lcd->locate(0,0);
+ i2c1.frequency(100000);//修改为100000，否则报错
+ i2c2.frequency(100000);
+
+ char id_regval[1] = {146};
+ char data[1] = {0};
+ 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};
+ i2c1.write(sensor_addr,timing_register,2,false);
+ i2c2.write(sensor_addr,timing_register,2,false);
+
+ char control_register[2] = {143,0};
+ i2c1.write(sensor_addr,control_register,2,false);
+ i2c2.write(sensor_addr,control_register,2,false);
+
+ char enable_register[2] = {128,3};
+ 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_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_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_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_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
+// while(1)
+// {
+// if(STM2Android.readable())
+// {
+ STM2Android.scanf("%s",&Android_0);//要给上位机传输dist以及用户识别
+// break;
+// }
+// }
+wait(2);
+ 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(1);
+ // 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);//得到上位机传来的begin
+
+ if(strcmp(flag_Android,flag_Android_1)==0)
+ {
+ wait(1);
+ if ((red_value1>green_value1)&&(red_value1>blue_value1)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("a3");//red+3
+ goal_1=3;
+ }
+ else if ((green_value1>red_value1)&&(green_value1>blue_value1)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("a2");//green+2
+ goal_1=2;
+ }
+ else if ((blue_value1>green_value1)&&(blue_value1>red_value1)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("a1");//blue+1
+ goal_1=1;
+ }
+ wait(1);
+ d=0;j=0;k=0;//电机转动
+ wait(5);
+ d=1;j=1;k=1;//电机停止转动
+ wait(1);
+
+ if ((red_value2>green_value2)&&(red_value2>blue_value2)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("b3");//red+3
+ goal_2=3;
+ }
+ else if ((green_value2>red_value2)&&(green_value2>blue_value2)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("b2");//green+2
+ goal_2=2;
+ }
+ else if ((blue_value2>green_value2)&&(blue_value2>red_value2)) //shifoukeyiyizhichuanzhi???????????????
+ {
+ STM2Android.printf("b1");//blue+1
+ goal_2=1;
+ }
+ wait(1);
+
+ 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;
+ }
+ }
+
+ //
+
+ else 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;//电机停止转动
+// }
+// }
+ }//while循环结束
+
+}
\ No newline at end of file