File content as of revision 1:0c82ff54d168:

#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,9600);
I2C i2c(PC_1, PC_0); //pins for I2C communication (SDA, SCL)

DigitalOut d(D3);  //dianjide diyiwei
//DigitalOut j(D4);  //dianjide dierwei

DigitalOut a(PC_10);//hongwaide yi
DigitalOut b(PC_12);//hongwaide er

int sensor_addr = 41 << 1;
DigitalOut green(LED1);

char flag_Android[]="";
char flag_Android_1[]="begin";

int main()
{   
    int goal_1=0;
    int goal_2=0;
    d=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
    lcd = new WattBob_TextLCD(par_port); 
      
    
 
 while(1)
 {
         STM2Android.scanf("%s",&flag_Android);
         if(strcmp(flag_Android,flag_Android_1)==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，否则报错
            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
            char timing_register[2] = {129,0};
            i2c.write(sensor_addr,timing_register,2,false);
            char control_register[2] = {143,0};
            i2c.write(sensor_addr,control_register,2,false);    
            char enable_register[2] = {128,3};
            i2c.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 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 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 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???????????????
              {
                  STM2Android.printf("a2");//red+2
                  goal_1=2;
              }
            else if (green_value>red_value&&green_value>blue_value)     //shifoukeyiyizhichuanzhi???????????????
              {
                  STM2Android.printf("a1");//green+1
                  goal_1=1;
              }
            else if (red_value>green_value&&red_value>blue_value)     //shifoukeyiyizhichuanzhi???????????????
              {
                  STM2Android.printf("a0");//blue+0
                  goal_=0;
              }
            wait(5);
            
            
            
            
              
           d=0;//dianjizhuandong
           wait(10);
           d=1;//dianjitingzhizhuandong
           wait(2);
           
           
           
           if (red_value>green_value&&red_value>blue_value)     //shifoukeyiyizhichuanzhi???????????????
              {
                  STM2Android.printf("b2");//red+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???????????????
              {
                  STM2Android.printf("b0");//blue+0
                  goal_2=0;
              }
           if(goal_1>goal_2)
              
       //    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;
        
       
       // 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);
     
    // }
 }
}