Arduino寻迹与识别红绿灯

相关文章:
http://blog.csdn.net/Romantic_C/article/details/78676056

C语言中的关键字及常用语句:
http://blog.csdn.net/Romantic_C/article/details/78660805

常用C语言操作符：
http://blog.csdn.net/romantic_c/article/details/78235116

简单寻迹与使用识别红绿灯停车即颜色传感器 下面使用GY-33

代码如下：

//////////////////////////////////////////////////////////
/*GY-33
VCC----VCC
CT----A5
DR----A4
GND--GND
S0---GND*/
//////////////////////////////////////////////////////////

#include 
#include 
#include 

#ifndef F_CPU
#define F_CPU 16000000UL
#endif

#define SCL_CLOCK 50000L

#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !"
#endif


#define I2C_READ    1
#define I2C_WRITE   0
#define i2c_read(ack)  (ack) ? i2c_readAck() : i2c_readNak();
////////////////////////////////////////////////

#define uint16_t unsigned int //无符号整型 重命名为 uint16_t
typedef struct  //结构体
{
uint16_t Red;
uint16_t Green;
uint16_t Blue;
uint16_t Clear;
} RGB;

unsigned char Re_buf;
unsigned char sign = 0;
RGB rgb;
uint16_t CT = 0, Lux = 0, temp = 0;
int bug = 1, bugg = 0;
int i = 0, select = 0, p = 0, k = 0;
double  j = 0;
byte  color = 0, rgb_data[3] = { 0 }; 
///////////////////////////////////////////
int l_g = 13;       // 左前电机 前进
int l_b = 12;       // 后退

int ll_g = 9;      //左后电机  前进
int ll_b = 8;      //后退

int r_g = 4;       // 右前电机  前进
int r_b = 7;       //后退

int rr_g = 2;      //右后电机   前进
int rr_b = 3;      //后退

int l_pwm = 11;     // 左前电机 控速
int ll_pwm = 10;     // 左后

int r_pwm = 6;     // 左后电机 控速
int rr_pwm = 5;     // 右后
int on_off = 1;

const int x1 = A0;    // 红外传感 定义
const int x2 = A1;   //
const int x3 = A2;   //
const int x4 = A3;   //
int l1;       //
int l2;       //
int l3;       //
int l4;       //
int pd;       //
//////////////////////////////////////////////////////////
void setup() //定义每个口的模式
{

Serial.begin(115200);
i2c_init();
delay(1);

pinMode(l_g, OUTPUT);  
pinMode(ll_g, OUTPUT);
pinMode(l_b, OUTPUT);
pinMode(ll_b, OUTPUT);

pinMode(r_g, OUTPUT);
pinMode(rr_g, OUTPUT);
pinMode(r_b, OUTPUT);
pinMode(rr_b, OUTPUT);

pinMode(r_pwm, INPUT);
pinMode(rr_pwm, INPUT);
pinMode(r_pwm, INPUT);
pinMode(rr_pwm, INPUT);

pinMode(l1, INPUT);
pinMode(l2, INPUT);
pinMode(l3, INPUT);
pinMode(l4, INPUT);
pinMode(on_off, INPUT);

digitalWrite(l1, HIGH);
digitalWrite(l2, HIGH);
digitalWrite(l3, HIGH);
digitalWrite(l4, HIGH);
digitalWrite(on_off, HIGH);

}
//********************************************************

void i2c_init(void)
{

TWSR = 0;                        
TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2;  

}

//********************************************************
unsigned char i2c_start(unsigned char address)
{
uint8_t   twst;
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
while (!(TWCR & (1 << TWINT)));
twst = TW_STATUS & 0xF8;
if ((twst != TW_START) && (twst != TW_REP_START)) return 1;
TWDR = address;
TWCR = (1 << TWINT) | (1 << TWEN);
while (!(TWCR & (1 << TWINT)));
twst = TW_STATUS & 0xF8;
if ((twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK)) return 1;

return 0;
}

//********************************************************
void i2c_start_wait(unsigned char address)
{
    uint8_t   twst;
    while (1)
    {
      TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
      while (!(TWCR & (1 << TWINT)));
      twst = TW_STATUS & 0xF8;
      if ((twst != TW_START) && (twst != TW_REP_START)) continue;
      TWDR = address;
      TWCR = (1 << TWINT) | (1 << TWEN);
      while (!(TWCR & (1 << TWINT)));
      twst = TW_STATUS & 0xF8;
      if ((twst == TW_MT_SLA_NACK) || (twst == TW_MR_DATA_NACK))
        {
          TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
          while (TWCR & (1 << TWSTO));
          continue;
        }
        break;
    }
}

//********************************************************
unsigned char i2c_rep_start(unsigned char address)
{
return i2c_start(address);
}

//********************************************************
void i2c_stop(void)
{
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
while (TWCR & (1 << TWSTO));
}
//********************************************************
unsigned char i2c_write(unsigned char data)
{
uint8_t   twst;
TWDR = data;
TWCR = (1 << TWINT) | (1 << TWEN);
while (!(TWCR & (1 << TWINT)));
twst = TW_STATUS & 0xF8;
if (twst != TW_MT_DATA_ACK) return 1;
return 0;

}


//********************************************************
unsigned char i2c_readAck(void)
{
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);
while (!(TWCR & (1 << TWINT)));
return TWDR;
}


//********************************************************
unsigned char i2c_readNak(void)
{
TWCR = (1 << TWINT) | (1 << TWEN);
while (!(TWCR & (1 << TWINT)));
return TWDR;
}
//////////////////////////////////////////////////////////

void Go (int l_speed, int ll_speed, int r_speed, int rr_speed)    // 向前走
{
digitalWrite(l_g, HIGH);
digitalWrite(l_b, LOW);
analogWrite(l_pwm, l_speed);

digitalWrite(ll_g, HIGH);
digitalWrite(ll_b, LOW);
analogWrite(ll_pwm, ll_speed);

digitalWrite(r_g, HIGH);
digitalWrite(r_b, LOW);
analogWrite(r_pwm, r_speed);

digitalWrite(rr_g, HIGH);
digitalWrite(rr_b, LOW);
analogWrite(rr_pwm, rr_speed);

}
//////////////////////////////////////////////////////////
void Turn_Left (int l_speed, int ll_speed, int r_speed, int rr_speed)    // 90度左转弯
{
digitalWrite(l_g, LOW);
digitalWrite(l_b, HIGH);
analogWrite(l_pwm, l_speed);

digitalWrite(ll_g, LOW);
digitalWrite(ll_b, HIGH);
analogWrite(ll_pwm, ll_speed);

digitalWrite(r_g, HIGH);
digitalWrite(r_b, LOW);
analogWrite(r_pwm, r_speed);

digitalWrite(rr_g, HIGH);
digitalWrite(rr_b, LOW);
analogWrite(rr_pwm, rr_speed);

}
//////////////////////////////////////////////////////////
void Back (int l_speed, int ll_speed, int r_speed, int rr_speed)    // 倒车
{
digitalWrite(l_g, LOW);
digitalWrite(l_b, HIGH);

digitalWrite(ll_g, LOW);
digitalWrite(ll_b, HIGH);

analogWrite(l_pwm, l_speed);
analogWrite(ll_pwm, ll_speed);



digitalWrite(r_g, LOW);
digitalWrite(r_b, HIGH);

digitalWrite(rr_g, LOW);
digitalWrite(rr_b, HIGH);

analogWrite(r_pwm, r_speed);
analogWrite(rr_pwm, rr_speed);
}
//////////////////////////////////////////////////////////
void loop()
{
  pd = digitalRead(on_off);
  if (pd == HIGH)
  {
    select = 1;
  }
  switch (select)
  {
    case 1:   //////////////////////// 数字接口1不接任何地方 默认高电平 
    {

      unsigned char data[9] = { 0 };
      if (!sign)
      {

      //////////////////////////////////////////////////
      //原始 RGBC 计算                  即 Byte2=0x15 时

      iic_read(0x00, data, 8);
      rgb.Red = (data[0] << 8) | data[1];
      rgb.Green = (data[2] << 8) | data[3];
      rgb.Blue = (data[4] << 8) | data[5];
      rgb.Clear = (data[6] << 8) | data[7];
      //   Serial.print("Red: ");
      //   Serial.print(rgb.Red);
      //   Serial.print(",Green: ");
      //   Serial.print( rgb.Green);
      //   Serial.print(",Blue");
      //   Serial.print( rgb.Blue);
      //   Serial.print(",Clear");
      //   Serial.println(rgb.Clear);

      ///////////////////////////////////////////////////  
      //亮度、色温、简单颜色的计算方法   即 Byte2=0x25 时

      iic_read(0x08, data, 4);
      Lux = (data[0] << 8) | data[1];
      CT = (data[2] << 8) | data[3];
      temp = (data[4] << 8) | data[5];
      //  Serial.print("CT:");
      //  Serial.print(CT);
      //  Serial.print(",Lux:");
      //  Serial.println( Lux);

      ///////////////////////////////////////////////////
      //处理后的 RGB 值计算方法          即 Byte2=0x45 时

      iic_read(0x0c, data, 3);
      rgb_data[0] = data[0];
      rgb_data[1] = data[1];
      rgb_data[2] = data[2];
      Serial.print("r:");
      Serial.print(rgb_data[0]);
      Serial.print(",g:");
      Serial.print(rgb_data[1]);
      Serial.print(",b:");
      Serial.print(rgb_data[2]);

      /////////////////////////////////////////////////////////////  主要判别方式 其他辅助 确保安全
      //哪位置 1 表示该位的颜色    具体在arduino软件上打印串口输出  举例：绿色为8  红色为4
      //////color  bit7  bit6  bit5  bit4  bit3  bit2  bit1  bit0
      ////
      ///// 颜色   蓝色 深蓝色 绿色  黑色  白色  粉色  黄色  红色

      iic_read(0x0f, data, 1);
      color = data[0];
      Serial.print(",color:");
      Serial.println(color, HEX);

      ///////////////////////////////////////////////////
      ////小车跑n      



      if (bug && (bugg || (8 == color) && (rgb_data[1] >= 180))) // 理论绿色 G值为255
      //bugg 识别绿色灯后 使此函数一直可以被调用   
      {
         bugg = 1;
          for(;k<1;k++)    // 冲出其所设定的无黑带区
          {
           Go(255,255,255,255);
           delay(400);
      }
      l1 = digitalRead(x1);    //灯灭  HIGH 没扫到黑带
      l2 = digitalRead(x2);
      l3 = digitalRead(x3);
      l4 = digitalRead(x4);
       // 下列参数根据跑直线速度  自行设定 电机不可能没有相对误差 相同的电压可能转速不同
        if (l2 == LOW && l1 == HIGH  && l3 == HIGH && l4 == HIGH)   // 左 一灯 半圆弧
        {
         Go  (60,70,243,245);
        }

        if (l2 == HIGH && (!(l1  && l3)) && l4 == HIGH)   //直走
        {
         Go ( 150,150,150,150 );
        }

        if (l2 == LOW && l1 == LOW && l3 == LOW && l4 == HIGH)    //左 90度 转弯 
        {
          // Back ( 200, 200, 200, 200 ); 速度过快时可设定使电机反转 倒退 然后转弯 防止冲出赛道
          // delay(200);
           Turn_Left (200,200,180,180) ;
           delay (400);   //   1000是一秒
          // p=1;  用法下面有介绍
        }

        if (l2 == LOW && l1 == LOW && l3 == HIGH && l4 == HIGH)   //左微转 双灯
        {
         Go ( 175,175,150,150 ) ; 
        }

        if (l2 == HIGH && l1 == HIGH  && l3 == LOW && l4 == LOW)    //右微转   双灯  
       {
         Go ( 150,150,175,175 ) ; 
       }

        if (l2 == HIGH && l1 == HIGH && l3 == HIGH && l4 == LOW)   //右 一灯 转
        {
         Go ( 150,150,185,185 ) ; 
        }

        if (l2 == HIGH && l1 == HIGH && l3 == HIGH && l4 == HIGH)   //全部没扫到
        {
         Go ( 80,80,80,80 ) ;//存在可能 速度小点 使其运动一下 检测黑带 防止静止   
        }

       if (l2 == LOW && l1 == LOW  && l3 == LOW  && l4 == LOW)   // 四路全扫到 可能为90度弯 自行设定
       {
        ;
       }
       if (l2 == LOW && l1 == LOW  && l3 == LOW  && l4 == LOW)
         {
          //Back ( 200, 220, 250, 255 );
         // delay(100);
          Turn_Left (200,200,180,180) ;
          delay (400);   //   1000是一秒
        //  i=1;  用法下面有介绍     
         }

      } //endif 绿灯条件




      if ((4 == color ) && (rgb_data[0]  > 220)  ) // 红色R值 理论为255
      {
        bug = 0;
       // Back (200,200,200,200) ;
       // delay(100);
        Go (0,0,0,0) ;
       }

 //再不违反赛题的可能下，可用此相关变量实现可控距离停车


        /*if(i == 1 || p == 1) 
        {
          j--;
            if(j == 0 )
             {
                while(1)
                   {
                      zou (0 , 0 , 0 , 0 ) ;
                   }
              }
        }*/

    } //endif sign

    if (sign == 1)
    {
      iic_read(0x10, &data[8], 1);
      i2c_start_wait(0xb4);
      i2c_write(0x10);
      i2c_write(0x31);
      i2c_write((data[8] | 0x01));
      i2c_stop();
      sign = 3;
    }
    delay(200);

  }// case1
 break;
 /////////////////////////////////////////////////////////

  default: ///////////////////////////////////数字1接口接地
  {
   //  其他模式  自行设计  尽量不违反参赛规定  

    ;

  }//default

 }//switch

}//loop

void iic_read(unsigned char add, unsigned char *data, unsigned char len)
{
  i2c_start_wait(0xb4); //  将一个字节发送到设备数据字节进行传输
  i2c_write(add);      //  从器件读取一个字节，请求器件提供更多数据       1 write failed
  i2c_start_wait(0xb5);//  将一个字节发送到设备数据字节进行传输         0 write successful  
  while (len - 1)
  {
    *data++ = i2c_readAck();// 从器件读取一个字节后，出现stop条件(1继续 0停止)
    len--;
  }
  *data = i2c_readNak();  //
  i2c_stop();           //发出启动条件并发送地址和传输方向
}

void serialEvent() {
  while (Serial.available()) {
    Re_buf = (unsigned char)Serial.read();
    if (Re_buf == 'a')
      sign = 0;
    if (Re_buf == 'b')
      sign = 1;
    Re_buf = 0;
  }
}