|
发表于 2013-3-31 02:42:14
|
显示全部楼层
Randy 发表于 2013-3-29 10:00
看一下你整体的程序控制,这些都是数字控制的没什么难度的!
#include <TimerOne.h> //引用TimerOne.h 库文件
#include <Servo.h> //舵机头文件
Servo myservo;
#define S0 6 //定义S0 为引脚6
#define S1 5 //定义S1 为引脚5
#define S2 4 //定义S2 为引脚4
#define S3 3 //定义S3 为引脚3
#define OUT 2 //定义OUT 为引脚2
int g_count = 0; // 定义整型变量g_count 并赋初值为0,用于存储计数频率
int g_array[3]; // 定义整型数组变量g_array[3],用于存储RGB 的值
int g_flag = 0; // 定义整形变量g_flag 并赋初值为0,用于过滤器排列
float g_SF[3]; // 定义浮点型数组变量g_SF[3],用于存储RGB 比例因子
int value[3]; // 定义定义整型数组变量value[3],用于判断颜色
int IN1 = 32;
int IN2 = 33;
int IN3 = 36;
int IN4 = 37;
int IN5 = 38;
int IN6 = 39;
int IN7 = 42;
int IN8 = 43;
int ENA = 7;
int ENB = 10;
int ENC = 9;
int END = 8;
int val1=140;
int val2=140; //speed
int val5=100;
int val6=100;
int val3=50;
int val4=0;
int red;
int blue;
int yellow;
int black;
int white;
void advance(char a,char b) //前进
{
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
digitalWrite(IN5,HIGH);
digitalWrite(IN6,LOW);
digitalWrite(IN7,HIGH);
digitalWrite(IN8,LOW);
analogWrite(ENA,a); //PWM 调速
analogWrite(ENB,a); //PWM 调速
analogWrite(ENC,b); //PWM 调速
analogWrite(END,b); //PWM 调速
}
void back (char a,char b) //后退
{
digitalWrite(IN2,HIGH);
digitalWrite(IN1,LOW);
digitalWrite(IN4,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN6,HIGH);
digitalWrite(IN5,LOW);
digitalWrite(IN8,HIGH);
digitalWrite(IN7,LOW);
analogWrite(ENA,a); //PWM 调速
analogWrite(ENB,a); //PWM 调速
analogWrite(ENC,b); //PWM 调速
analogWrite(END,b); //PWM 调速
}
void turn_L (char a,char b) //左转
{
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
digitalWrite(IN5,HIGH);
digitalWrite(IN6,LOW);
digitalWrite(IN7,HIGH);
digitalWrite(IN8,LOW);
analogWrite(ENA,a); //PWM 调速
analogWrite(ENB,a); //PWM 调速
analogWrite(ENC,b); //PWM 调速
analogWrite(END,b); //PWM 调速
}
void turn_L90 (char a,char b) //左转
{
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
digitalWrite(IN5,HIGH);
digitalWrite(IN6,LOW);
digitalWrite(IN7,HIGH);
digitalWrite(IN8,LOW);
analogWrite(ENA,a); //PWM 调速
analogWrite(ENB,a); //PWM 调速
analogWrite(ENC,b); //PWM 调速
analogWrite(END,b); //PWM 调速
}
void turn_R (char a,char b) //右转
{
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
digitalWrite(IN5,HIGH);
digitalWrite(IN6,LOW);
digitalWrite(IN7,HIGH);
digitalWrite(IN8,LOW);
analogWrite(ENA,a); //PWM 调速
analogWrite(ENB,a); //PWM 调速
analogWrite(ENC,b); //PWM 调速
analogWrite(END,b); //PWM 调速
}
void turn_R90 (char a,char b) //右转
{
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
digitalWrite(IN5,LOW);
digitalWrite(IN6,HIGH);
digitalWrite(IN7,LOW);
digitalWrite(IN8,HIGH);
analogWrite(ENA,a); //PWM 调速
analogWrite(ENB,a); //PWM 调速
analogWrite(ENC,b); //PWM 调速
analogWrite(END,b); //PWM 调速
}
void Stop(void)
{
digitalWrite(IN1,LOW);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,LOW);
digitalWrite(IN5,LOW);
digitalWrite(IN6,LOW);
digitalWrite(IN7,LOW);
digitalWrite(IN8,LOW);
for(int j=0;j<2;j++)
{
delay(1000);
}
}
// 初始化tsc230 和设置频率
void TSC_Init()
{
pinMode(S0, OUTPUT); //定义S0 为输出状态
pinMode(S1, OUTPUT); //定义S1 为输出状态
pinMode(S2, OUTPUT); //定义S2 为输出状态
pinMode(S3, OUTPUT); //定义S3 为输出状态
pinMode(OUT, INPUT); //定义OUT 为输入状态
digitalWrite(S0, LOW); //定义S0 为低电平
digitalWrite(S1, HIGH); //定义S1 为高电平
//输出频率缩放2%
}
// 选择过滤器的颜色
void TSC_FilterColor(int Level01, int Level02)
{
if(Level01 != 0) //如果Level01 不等于0
Level01 = HIGH; //则Level01 为高电平
if(Level02 != 0) //如果Level02 不等于0
Level02 = HIGH; //则Level02 为高电平
digitalWrite(S2, Level01); //将Level01 值送给S2
digitalWrite(S3, Level02); //将Level02 值送给S3
//选择过滤器颜色
}
void TSC_Count()
{
g_count ++ ; //自动计算频率
}
void TSC_Callback()
{
switch(g_flag)
{ case 0: Serial.println("->WB Start"); //串口打印字符串"->WB Start"
TSC_WB(LOW, LOW); //没有过滤红色
break;
case 1: Serial.print("->Frequency R="); //串口打印字符串"->Frequency R="
Serial.println(g_count); //串口打印g_count 变量值
g_array[0] = g_count;
TSC_WB(HIGH, HIGH); //没有过滤绿色
break;
case 2: Serial.print("->Frequency G="); //串口打印字符串"->Frequency G="
Serial.println(g_count); //串口打印g_count 变量值
g_array[1] = g_count;
TSC_WB(LOW, HIGH); //没有过滤蓝色
break;
case 3: Serial.print("->Frequency B="); //串口打印字符串"->Frequency B="
Serial.println(g_count); //串口打印g_count 变量值
Serial.println("->WB End"); //串口打印字符串"->WB End"
g_array[2] = g_count;
TSC_WB(HIGH, LOW); //清除(无过滤)
break;
default: g_count = 0;
break;
}
}
void TSC_WB(int Level0, int Level1) //白平衡
{
g_count = 0;
g_flag ++;
TSC_FilterColor(Level0, Level1);
Timer1.setPeriod(1000000); //设置一秒周期
}
void setup()
{
myservo.attach(46);
myservo.write(57);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(IN5, OUTPUT);
pinMode(IN6, OUTPUT);
pinMode(IN7, OUTPUT);
pinMode(IN8, OUTPUT);
pinMode(22,INPUT);
pinMode(23,INPUT);
pinMode(26,INPUT);
pinMode(27,INPUT);
TSC_Init(); //初始化tcs230
Serial.begin(9600); //打开串口并设置通信波特率为9600
Timer1.initialize(); //初始化默认是一秒
Timer1.attachInterrupt(TSC_Callback); //外部中断为一秒
attachInterrupt(0, TSC_Count, RISING); //外部中断口初始0
delay(4000); //延迟4 秒
for(int i=0; i<3; i++)
Serial.println(g_array); //串口打印g_array变量值
g_SF[0] = 255.0/ g_array[0]; //红色的比例因子
g_SF[1] = 255.0/ g_array[1] ; //绿色的比例因子
g_SF[2] = 255.0/ g_array[2] ; //蓝色的比例因子
Serial.println(g_SF[0]); //串口打印g_SF[0]变量值
Serial.println(g_SF[1]); //串口打印g_SF[1]变量值
Serial.println(g_SF[2]); //串口打印g_SF[2]变量值
}
void loop()
{
for(int k=0;k<700;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<2400;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop();
turn_L90(val1,val2); //90
delay(2000);
Stop();
for(int k=57;k>=45;k=k-1)
{
myservo.write(k);
delay(50);
}
for(int k=0;k<1460;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
Stop();
for(int j=45;j<=57;j=j+1)
{
myservo.write(j);
delay(50);
}
digitalWrite(13,HIGH);
red=0; yellow=0; black=0; white=0; blue=0;
for(int i=0;i<6;i++)
{
g_flag = 0;
for(int i=0; i<3; i++)
{
Serial.println(int(g_array * g_SF));
value=int(g_array * g_SF); //将g_array * g_SF值赋值给value
}
if ((value[0] -value[1]>15)&&(value[0] -value[2]>15)) //如果变量value数值满足为红 色值范围则执行下面语句
{ Serial.println("->Red"); red=red+1;}//串口打印字符串"->Red"
else if ((value[1] -value[2]>22)&&(value[0] -value[2]>20)) //如果变量value数值满足为黄色值范围则执行下面语句
{ Serial.println("->Yellow"); yellow=yellow+1;} //串口打印字符串"->Yellow"
else if (((value[0]>46)&&(value[0]<80)) && ((value[1]>54)&&(value[1]<87)) && ((value[2]>54) && (value[2]<87))) //如果变量value数值满足为黑色值范围则执行下面语句
{ Serial.println("->Black"); black=black+1;} //串口打印字符串"->Black"
else if (((value[0]>105)&&(value[0]<123))&&((value[1]>112)&&(value[1]<142))&&((value[2]>120)&&(value[2]<149))) //如果变量value数值满足为蓝色值范围则执行下面语句
{ Serial.println("->Blue"); blue=blue+1;}//串口打印字符串"->Blue"
else if (((value[0]>123)&&(value[1]>145))&&(value[2]>143)) //如果变量value数值满足为白色值范围则执行下面语句
{ Serial.println("->White"); white=white+1;}//串口打印字符串"->White"
else if (value[0]>0 && value[1]>0 && value[2]>0) //如果变量value数值不满足上述颜色值范围则执行下面语句
{ Serial.println("->Other Color");} //串口打印字符串"->Other Color"
delay(1000); //延迟1 秒
}
if ((yellow>=blue)&&(yellow>=red)&&(yellow>=white)&&(yellow>=black))
{
for(int k=0;k<1800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //黄色位置
delay(200);
for(int j=58;j>=45;j=j-1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<600;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int k=45;k<=58;k=k+1)
{
myservo.write(k);
delay(50);
}
delay(100);
for(int k=0;k<1800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过180度角
delay(3200);
Stop();
delay(1000);
}
else if((white>=blue)&&(white>=red)&&(white>=black)&&(white>=yellow))
{ //如果是白色
for(int k=0;k<1400;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过45度角
delay(980);
Stop(); delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<2200;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //白色位置
for(int j=57;j>=45;j=j-1)
{
myservo.write(j);
delay(50);
}
for(int k=0;k<1600;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int k=45;k<=57;k=k+1)
{
myservo.write(k);
delay(50);
}
for(int k=0;k<1500;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过135度角
delay(2700);
Stop();
delay(1000);
}
else if((red>=blue)&&(red>=white)&&(red>=black)&&(red>=yellow))
{ //如果是红色
for(int k=0;k<1400;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过45度角
delay(980);
Stop();
delay(200);
for(int k=0;k<1480;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<1780;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
} //先到达白色位置,然后右转112.5度
Stop();
delay(200);
turn_R90(val1,val2);
delay(2300);
Stop();
delay(200);
for(int k=0;k<2200;k++) //外围黑线
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))||(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //已到达红色位置
delay(200);
for(int k=58;k>=45;k=k-1)
{
myservo.write(k);
delay(50);
}
for(int k=0;k<2340;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(100);
for(int j=45;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
turn_R90(val1,val2); //转回主轴线
delay(1400);
Stop();
delay(200);
for(int k=0;k<2900;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))||(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); delay(200);
turn_L90(val1,val2); //转过45度角
delay(980);
Stop(); delay(1000);
}
else if((black>=blue)&&(black>=red)&&(black>=white)&&(black>=yellow))
{
for(int k=0;k<1300;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过135度角
delay(2800);
Stop(); delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<2200;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //黑色位置
for(int j=57;j>=45;j=j-1)
{
myservo.write(j);
delay(50);
}
for(int k=0;k<1600;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int k=45;k<=57;k=k+1)
{
myservo.write(k);
delay(50);
}
for(int k=0;k<1500;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过45度角
delay(980);
Stop();
delay(1000);
}
else if((blue>=white)&&(blue>=red)&&(blue>=black)&&(blue>=yellow))
{
for(int k=0;k<1360;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_R90(val1,val2); //转过135度角
delay(2900);
Stop();
delay(200);
for(int k=0;k<1400;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<750;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
} //先到达黑色位置,然后右转112.5度
Stop();
delay(200);
turn_R90(val1,val2);
delay(2350);
Stop();
delay(200);
for(int k=0;k<2000;k++) //外围黑线
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))||(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //已到达蓝色位置
delay(200);
for(int k=58;k>=45;k=k-1)
{
myservo.write(k);
delay(50);
}
for(int k=0;k<2200;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(100);
for(int j=45;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
turn_L90(val1,val2); //转回主轴线
delay(2400);
Stop();
delay(200);
for(int k=0;k<3100;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))||(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); delay(200);
turn_R90(val1,val2); //转过45度角
delay(980);
Stop(); delay(1000);
}
/////////////////////////判断二//////////////////////////
for(int j=58;j<=45;j=j-1)
{
myservo.write(j);
delay(50);
}
for(int k=0;k<1460;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
Stop();
delay(200);
for(int j=45;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
g_flag = 0;
red=0; yellow=0; black=0; white=0; blue=0;
for(int i=0; i<3; i++)
{
Serial.println(int(g_array * g_SF));
value=int(g_array * g_SF); //将g_array * g_SF值赋值给value
}
if ((value[0] -value[1]>15)&&(value[0] -value[2]>15)) //如果变量value数值满足为红 色值范围则执行下面语句
{ Serial.println("->Red"); red=red+1;}//串口打印字符串"->Red"
else if ((value[1] -value[2]>22)&&(value[0] -value[2]>20)) //如果变量value数值满足为黄色值范围则执行下面语句
{ Serial.println("->Yellow"); yellow=yellow+1;} //串口打印字符串"->Yellow"
else if (((value[0]>46)&&(value[0]<80)) && ((value[1]>54)&&(value[1]<87)) && ((value[2]>54) && (value[2]<87))) //如果变量value数值满足为黑色值范围则执行下面语句
{ Serial.println("->Black"); black=black+1;} //串口打印字符串"->Black"
else if (((value[0]>105)&&(value[0]<123))&&((value[1]>112)&&(value[1]<142))&&((value[2]>120)&&(value[2]<149))) //如果变量value数值满足为蓝色值范围则执行下面语句
{ Serial.println("->Blue"); blue=blue+1;}//串口打印字符串"->Blue"
else if (((value[0]>123)&&(value[1]>145))&&(value[2]>143)) //如果变量value数值满足为白色值范围则执行下面语句
{ Serial.println("->White"); white=white+1;}//串口打印字符串"->White"
else if (value[0]>0 && value[1]>0 && value[2]>0) //如果变量value数值不满足上述颜色值范围则执行下面语句
{ Serial.println("->Other Color");} //串口打印字符串"->Other Color"
delay(1000); //延迟1 秒
if ((blue>=red)&&(blue>=white)&&(blue>=black)&&(blue>=yellow)) //蓝色
{
for(int k=0;k<1800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); delay(200);
for(int j=58;j>=46;j=j-1)
{
myservo.write(j);
delay(50);
}
delay(200); //放置在蓝色区域
for(int k=0;k<1000;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int j=45;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<1800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop();
turn_L90(val1,val2);//转过90度角
delay(2000); Stop(); delay(1000);
}
else if((black>=blue)&&(black>=red)&&(black>=white)&&(black>=yellow))
{
for(int k=0;k<1360;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_L90(val1,val2); //左转过45度角
delay(980);
Stop(); delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<2200;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //黑色位置
delay(200);
for(int j=58;j>=46;j=j-1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int j=46;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<3100;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); delay(200);
turn_L90(val1,val2); //左转过45度角
delay(980);
Stop(); delay(1000);
}
else if((yellow>=blue)&&(yellow>=red)&&(yellow>=white)&&(yellow>=black))
{
for(int k=0;k<1400;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_L90(val1,val2); //左转过180度角
delay(3600);
Stop(); delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<2200;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //黄色位置
delay(200);
for(int j=58;j>=46;j=j-1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int j=46;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<3100;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); delay(200);
turn_R90(val1,val2); //右转过90度角
delay(2000);
Stop(); delay(1000);
}
else if((white>=blue)&&(white>=red)&&(white>=yellow)&&(white>=black)) //判定为白色
{
for(int k=0;k<1400;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop();
delay(200);
turn_L90(val1,val2); //左转过135度角
delay(2700);
Stop(); delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val5,val6);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
}
for(int k=0;k<2200;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
advance(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
turn_L(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
turn_R(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); //白色位置
delay(200);
for(int j=58;j>=46;j=j-1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<800;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
}
Stop(); delay(200);
for(int j=45;j<=58;j=j+1)
{
myservo.write(j);
delay(50);
}
delay(200);
for(int k=0;k<3100;k++)
{
if ((HIGH==digitalRead(22))&&(HIGH==digitalRead(23)))
{
back(val1,val2);
delay(1);
}
else if (LOW==digitalRead(22))
{
back(val3,val1);
delay(1);
}
else if (LOW==digitalRead(23))
{
back(val1,val3);
delay(1);
}
else if ((LOW==digitalRead(26))&&(LOW==digitalRead(27)))
{
Stop();
break;
}
}
Stop(); delay(200);
turn_R90(val1,val2); //右转过45度角
delay(980);
Stop(); delay(1000);
}
}
这是我参加校内一个分拣比赛的程序,主题思想就是到位置识别颜色,然后分拣到对应位置,麻烦楼主啦。。。我用的是Arduino的MFGA2560.。。 |
|