Servo_Move_Control.ino
//===================舵机========================舵机=======================舵机=======================舵机===========================舵机==========================舵机==============================舵机==============================舵机
//====舵机===========================舵机======================舵机=======================舵机=========================舵机==========================舵机============================舵机==================================舵机=============
//===================舵机========================舵机=======================舵机=======================舵机===========================舵机==========================舵机==============================舵机==============================舵机
void Servo_Init()
{
for(int i=0;i<servo_num;i++)
{
myServo[i].attach(servo_port[i]);
myServo[i].write(map(value_init[i],0,180,500,2500));
delay(20);
}
}
void ServoStop(int which){//释放舵机
myServo[which].detach();
digitalWrite(servo_port[which],LOW);
}
void ServoGo(int which , float where){//打开并给舵机写入相关角度
if(where!=200){
if(where==201) ServoStop(which);
else{
myServo[which].write(map(where,0,180,500,2500));
}
}
}
void Servo_Move_Single(int which,int Start_angle,int End_angle,unsigned long Servo_move_time)
{
int servo_flags = 0;
int delta_servo_angle = abs(Start_angle-End_angle);
if( (Start_angle - End_angle)<0 )
{
servo_flags = 1;
}
else{ servo_flags = -1; }
for(int i=0;i<delta_servo_angle;i++)
{
myServo[which].write(map( Start_angle+(servo_flags*i) ,0,180,500,2500));
delay(Servo_move_time);
}
}
void servo_move(float value0, float value1, float value2,int delaytimes){ //舵机动作函数
float value_arguments[] = {value0, value1, value2};
float value_delta[servo_num];
for(int i=0;i<servo_num;i++){
value_delta[i] = (value_arguments[i] - value_init[i]) / f;
}
for(int i=0;i<f;i++){
for(int k=0;k<servo_num;k++){
value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];
}
for(int j=0;j<servo_num;j++){
ServoGo(j,value_init[j]);
}
delay(delaytimes/f);
}
}
Color_detection.ino
/*
* 黄--------S3
* 橙--------S2
* 红--------GND
* 棕--------VCC
* 黑--------OUT
* 白--------LED
* 灰--------S1
* 紫--------S0
*/
//TCS230连接设置
const int s0 = 2;
const int s1 = 4;
const int s2 = 7;
const int s3 = 11;
const int out = 14;
const int led = 15;
// Variables
int red = 0;
int green = 0;
int blue = 0;
void Color_Init()
{
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
pinMode(led, OUTPUT);
digitalWrite(s0, HIGH);
digitalWrite(s1, HIGH);
}
/*
* color_judge[0] red green
* color_judge[0] green red
*/
void return_color_ballon()
{
digitalWrite(led, HIGH);
int numbers_count = 0;
int color_judge[12]={0,0,0,0,0,0,0,0,0,0,0,0};
int red_summer,green_summer,blue_summer;
Serial.println("---------------Start---------------");
unsigned long time_now = millis();
while( (millis() - time_now ) < 2000)
{
numbers_count ++;
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
//count OUT, pRed, RED
red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //blue
digitalWrite(s3, HIGH);
//count OUT, pBLUE, BLUE
blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //red
digitalWrite(s2, HIGH);
//count OUT, pGreen, GREEN
green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //green
if(red<blue){color_judge[0] = color_judge[0] +1;} else{color_judge[1] = color_judge[1] +1;}
if(red<green){color_judge[2] = color_judge[2] +1;} else{color_judge[3] = color_judge[3] +1;}
if(blue<red){color_judge[4] = color_judge[4] +1;} else{color_judge[5] = color_judge[5] +1;}
if(blue<green){color_judge[6] = color_judge[6] +1;} else{color_judge[7] = color_judge[7] +1;}
if(green<red){color_judge[8] = color_judge[8] +1;} else{color_judge[9] = color_judge[9] +1;}
if(green<blue){color_judge[10] = color_judge[10] +1;} else{color_judge[11] = color_judge[11] +1;}
//输出检测红色色卡的信息
// Serial.print("color[0]=");Serial.print(color_judge[0]);Serial.print(" | ");
// Serial.print("color[1]=");Serial.print(color_judge[1]);Serial.print(" | ");
// Serial.print("color[2]=");Serial.print(color_judge[2]);Serial.print(" | ");
// Serial.print("color[3]=");Serial.print(color_judge[3]);Serial.print(" | ");
//输出检测蓝色色卡的信息
// Serial.print("color[4]=");Serial.print(color_judge[4]);Serial.print(" | ");
// Serial.print("color[5]=");Serial.print(color_judge[5]);Serial.print(" | ");
// Serial.print("color[6]=");Serial.print(color_judge[6]);Serial.print(" | ");
// Serial.print("color[7]=");Serial.print(color_judge[7]);Serial.print(" | ");
// Serial.print("color[12]=");Serial.print(color_judge[12]);Serial.print(" | ");
// Serial.print("color[13]=");Serial.print(color_judge[13]);Serial.print(" | ");
//输出绿色色卡的信息
// Serial.print("color[8]=");Serial.print(color_judge[8]);Serial.print(" | ");
// Serial.print("color[9]=");Serial.print(color_judge[9]);Serial.print(" | ");
// Serial.print("color[10]=");Serial.print(color_judge[10]);Serial.print(" | ");
// Serial.print("color[11]=");Serial.print(color_judge[11]);Serial.print(" | ");
//输出red，green，blue色域值范围
// Serial.print(numbers_count);Serial.print(": ");Serial.print("red:");Serial.print(red);Serial.print(" | ");
// Serial.print("blue:");Serial.print(blue);Serial.print(" | ");
// Serial.print("green:");Serial.println(green);
}
Serial.println();
if( (color_judge[0] > color_judge[1]) && ((color_judge[2] > color_judge[3])) )
// if( (color_judge[0] > (numbers_count/2)) && (color_judge[2] > (numbers_count/2)) )
{
// Serial.println("The color is red");
color_detection_ballon = 1;
}
else if( (color_judge[4] > color_judge[5]) && ((color_judge[6] > color_judge[7])) )
{
// Serial.println("The color is blue");
color_detection_ballon = 2;
}
else if( (color_judge[8] > color_judge[9]) && ((color_judge[10] > color_judge[11])) )
{
// Serial.println("The color is green");
color_detection_ballon = 3;
}
// Serial.println("---------------End---------------");
// Serial.println();
digitalWrite(led, LOW);delay(500);
}
void return_color_card()
{
digitalWrite(led, HIGH);
int numbers_count = 0;
int color_judge[12]={0,0,0,0,0,0,0,0,0,0,0,0};
int red_summer,green_summer,blue_summer;
Serial.println("---------------Start---------------");
unsigned long time_now = millis();
while( (millis() - time_now ) < 2000)
{
numbers_count ++;
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
//count OUT, pRed, RED
red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //blue
digitalWrite(s3, HIGH);
//count OUT, pBLUE, BLUE
blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //red
digitalWrite(s2, HIGH);
//count OUT, pGreen, GREEN
green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //green
if(red<blue){color_judge[0] = color_judge[0] +1;} else{color_judge[1] = color_judge[1] +1;}
if(red<green){color_judge[2] = color_judge[2] +1;} else{color_judge[3] = color_judge[3] +1;}
if(blue<red){color_judge[4] = color_judge[4] +1;} else{color_judge[5] = color_judge[5] +1;}
if(blue<green){color_judge[6] = color_judge[6] +1;} else{color_judge[7] = color_judge[7] +1;}
if(green<red){color_judge[8] = color_judge[8] +1;} else{color_judge[9] = color_judge[9] +1;}
if(green<blue){color_judge[10] = color_judge[10] +1;} else{color_judge[11] = color_judge[11] +1;}
//输出检测红色色卡的信息
// Serial.print("color[0]=");Serial.print(color_judge[0]);Serial.print(" | ");
// Serial.print("color[1]=");Serial.print(color_judge[1]);Serial.print(" | ");
// Serial.print("color[2]=");Serial.print(color_judge[2]);Serial.print(" | ");
// Serial.print("color[3]=");Serial.print(color_judge[3]);Serial.print(" | ");
//输出检测蓝色色卡的信息
// Serial.print("color[4]=");Serial.print(color_judge[4]);Serial.print(" | ");
// Serial.print("color[5]=");Serial.print(color_judge[5]);Serial.print(" | ");
// Serial.print("color[6]=");Serial.print(color_judge[6]);Serial.print(" | ");
// Serial.print("color[7]=");Serial.print(color_judge[7]);Serial.print(" | ");
// Serial.print("color[12]=");Serial.print(color_judge[12]);Serial.print(" | ");
// Serial.print("color[13]=");Serial.print(color_judge[13]);Serial.print(" | ");
//输出绿色色卡的信息
// Serial.print("color[8]=");Serial.print(color_judge[8]);Serial.print(" | ");
// Serial.print("color[9]=");Serial.print(color_judge[9]);Serial.print(" | ");
// Serial.print("color[10]=");Serial.print(color_judge[10]);Serial.print(" | ");
// Serial.print("color[11]=");Serial.print(color_judge[11]);Serial.print(" | ");
//输出red，green，blue色域值范围
// Serial.print(numbers_count);Serial.print(": ");Serial.print("red:");Serial.print(red);Serial.print(" | ");
// Serial.print("blue:");Serial.print(blue);Serial.print(" | ");
// Serial.print("green:");Serial.println(green);
}
Serial.println();
if( (color_judge[0] > color_judge[1]) && ((color_judge[2] > color_judge[3])) )
// if( (color_judge[0] > (numbers_count/2)) && (color_judge[2] > (numbers_count/2)) )
{
// Serial.println("The color is red");
color_detection_card = 1;
}
else if( (color_judge[4] > color_judge[5]) && ((color_judge[6] > color_judge[7])) )
{
// Serial.println("The color is blue");
color_detection_card = 2;
}
else if( (color_judge[8] > color_judge[9]) && ((color_judge[10] > color_judge[11])) )
{
// Serial.println("The color is green");
color_detection_card = 3;
}
// Serial.println("---------------End---------------");
// Serial.println();
digitalWrite(led, LOW);delay(500);
}
void color()
{
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
//count OUT, pRed, RED
red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //blue
digitalWrite(s3, HIGH);
//count OUT, pBLUE, BLUE
blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //red
digitalWrite(s2, HIGH);
//count OUT, pGreen, GREEN
green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); //green
Serial.print("red:");Serial.print(red);Serial.print(" | ");
Serial.print("blue:");Serial.print(blue);Serial.print(" | ");
Serial.print("green:");Serial.println(green);
}
Motor_Gray_Control.ino
//灰度传感器初始化函数
void Motor_Sensor_Init()
{
for(int i=0;i<Car_Head_Gray_SensorPin_Num;i++) {//初始化灰度传感器
pinMode(Gray_SensorPin[i],INPUT);
delay(20);
}
for(int i=0;i<motor_num;i++) {//初始化 电机
pinMode(Car_DC_Motor_Pin[i],OUTPUT);
delay(20);
}
}
void Serialprint_gray_sensor_data_analogRead()
{
int data_sensor[3]={0,0,0};
for(int i=0;i<3;i++)
{
Serial.print(Gray_SensorPin[i]);Serial.print(": ");
Serial.print(analogRead(Gray_SensorPin[i]));
Serial.print(" | ");
}
Serial.println();
}
void Serialprint_gray_sensor_data()
{
int data_sensor[3]={0,0,0};
for(int i=0;i<3;i++)
{
Serial.print(Gray_SensorPin[i]);Serial.print(": ");
Serial.print(digitalRead(Gray_SensorPin[i]));
Serial.print(" | ");
}
Serial.println();
}
/*-----------------------------------------------------------------------
A2 A3 A4
----------------------------------
0 0 0 0x00 表示三个传感器都没有触发
0 0 1 0x01 表示小车右边一个传感器触发
0 1 0 0x02 表示小车中间传感器触发
0 1 1 0x03 表示小车右边两个传感器都触发
1 0 0 0x04 表示小车左边一个传感器触发
1 0 1 0x05
1 1 0 0x06 表示小车左边两个传感器都触发
1 1 1 0x07 表示小车三个传感器都触发
------------------------------------------------------------------------*/
int Detection_tracking() //灰度传感器A4,A3,A2,用来小车巡线时，返回传感器数值；
{
int num = 0;
for(int i=0;i<Car_Head_Gray_SensorPin_Num;i++)
{
num |= ( (!digitalRead(Gray_SensorPin[i]) ) << i);
}
Serial.println(num);
return num;
}
void Tracking_Automatic_Tracking(unsigned long delay_tracking_time)
{
unsigned long now_times = millis();
while(millis()-now_times<delay_tracking_time)
{
Automatic_Tracking();
}
}
void Automatic_Tracking() //小车前方两个灰度传感器用来寻迹（即小车巡线）。
{
int Get_Data = 0;
Get_Data = Detection_tracking();
switch(Get_Data)
{
case 0x00: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward
case 0x01: Car_Move(Right, Right_Left_Speed, Right_Right_Speed );break;//RIGHT
case 0x02: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward
case 0x03: Car_Move(Right, Right_Left_Speed, Right_Right_Speed );break;//right
case 0x04: Car_Move(Left, Left_Left_Speed, Left_Right_Speed );break;//left
case 0x06: Car_Move(Left, Left_Left_Speed, Left_Right_Speed );break;//LEFT
case 0x07: Gray_Three ++; Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward
default: break;
}
}
int Detection_tracking_analogRead() //灰度传感器A4,A3,A2,用来小车巡线时，返回传感器数值；
{
int num = 0;
int analogRead_data[3] = {0,0,0};
for(int i=0;i<Car_Head_Gray_SensorPin_Num;i++)
{
analogRead_data[i] = analogRead(Gray_SensorPin[i]);
if( analogRead_data[i] <=600 ){analogRead_data[i] = 1;} else{analogRead_data[i] = 0;}
num |= ( (analogRead_data[i]) << i);
}
Serial.println(num);
return num;
}
void Automatic_Tracking_analogRead() //小车前方两个灰度传感器用来寻迹（即小车巡线）。
{
int Get_Data = 0;
Get_Data = Detection_tracking_analogRead();
switch(Get_Data)
{
case 0x00: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward
case 0x01: Car_Move(Right, Right_Left_Speed, Right_Right_Speed );break;//RIGHT
case 0x02: Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward
case 0x03: Car_Move(Right, Right_Left_Speed, Right_Right_Speed );break;//right
case 0x04: Car_Move(Left, Left_Left_Speed, Left_Right_Speed );break;//left
case 0x06: Car_Move(Left, Left_Left_Speed, Left_Right_Speed );break;//LEFT
case 0x07: Gray_Three ++; Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);break;//forward
default: break;
}
}
void Car_Move(int Mode,int LeftSpeed,int RightSpeed)
{
switch(Mode)
{
case Forward:
analogWrite(Car_DC_Motor_Pin[0],LeftSpeed);
analogWrite(Car_DC_Motor_Pin[1],0);
analogWrite(Car_DC_Motor_Pin[2],RightSpeed);
analogWrite(Car_DC_Motor_Pin[3],0);
break;
case Back:
analogWrite(Car_DC_Motor_Pin[0],0);
analogWrite(Car_DC_Motor_Pin[1],LeftSpeed);
analogWrite(Car_DC_Motor_Pin[2],0);
analogWrite(Car_DC_Motor_Pin[3],RightSpeed);
break;
case Left:
analogWrite(Car_DC_Motor_Pin[0],0);
analogWrite(Car_DC_Motor_Pin[1],LeftSpeed);
analogWrite(Car_DC_Motor_Pin[2],RightSpeed);
analogWrite(Car_DC_Motor_Pin[3],0);
break;
case Right:
analogWrite(Car_DC_Motor_Pin[0],LeftSpeed);
analogWrite(Car_DC_Motor_Pin[1],0);
analogWrite(Car_DC_Motor_Pin[2],0);
analogWrite(Car_DC_Motor_Pin[3],RightSpeed);
break;
case Stop:
analogWrite(Car_DC_Motor_Pin[0],LeftSpeed);
analogWrite(Car_DC_Motor_Pin[1],LeftSpeed);
analogWrite(Car_DC_Motor_Pin[2],RightSpeed);
analogWrite(Car_DC_Motor_Pin[3],RightSpeed);
break;
}
}
void Car_Move_Test()
{
Car_Move(Forward,Forward_Left_Speed,Forward_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);
Car_Move(Back,Forward_Left_Speed,Forward_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);
Car_Move(Left,Left_Left_Speed, Left_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);
Car_Move(Right,Right_Left_Speed, Right_Right_Speed);delay(1500);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(1500);
}
Hit_Ballon_Car.ino
/*
*=====================================================================================================*
*实验接线: |
*=====================================================================================================*
* 车头
* 灰度传感器： A2 A3 A4
* *----------------*
* | |
* | |
* | |
* | | 右侧
* motor | | 车轮
* 9,10 | | 5，6
* | |
* | |
* | |
* | |
* | |
* *----------------*
* 车尾
* 舵机接线：
* 车头舵机:3
* 气球舵机:4
* 车尾舵机:8
*
*/
#include<ServoTimer2.h> //调用舵机库函数
ServoTimer2 myServo[3]; //声明舵机数组
#define TrackingSensorNum 3 //小车寻迹时使用的灰度传感器数量
#define Forward_Left_Speed 100 //小车前进时左轮速度
#define Forward_Right_Speed 90 //小车前进时右轮速度
#define Back_Left_Speed 110 //小车后退时左轮速度
#define Back_Right_Speed 90 //小车后退时右轮速度
#define Left_Left_Speed 235 //小车左转时左轮速度
#define Left_Right_Speed 240 //小车左转时右轮速度
#define Right_Left_Speed 235 //小车右转时左轮速度
#define Right_Right_Speed 240 //小车右转时右轮速度
#define Car_speed_stop 255
#define CAR_STOP_DELAY 500
int Gray_Three = 0;
int tracking_die = 0;
int color_detection_card = 10;
int color_detection_ballon = 0;
bool finish=true;
bool finish_all = true;
int Gray_SensorPin[3]={A4,A3,A2};//寻迹、检测路口传感器
int Car_Head_Gray_SensorPin_Num = sizeof(Gray_SensorPin)/sizeof(Gray_SensorPin[0]);//定义gray数量
int Car_DC_Motor_Pin[4] = {9,10,5,6};//直流电机引脚
int motor_num = sizeof(Car_DC_Motor_Pin) / sizeof(Car_DC_Motor_Pin[0]);//定义电机数量
int servo_port[3] = {3,8,12};//定义舵机引脚
int servo_num = sizeof(servo_port) / sizeof(servo_port[0]);//定义舵机数量
float value_init[3] = {30,5,165};//定义舵机初始角度
int f = 30; //定义舵机每个状态间转动的次数，以此来确定每个舵机每次转动的角度
enum{Forward=1,Back,Left,Right,Stop,ForwardSpeedDown,BackSpeedDown,ForwardRoad};//小车各种模式状态
void setup() {
// put your setup code here, to run once:
delay(1500);
Serial.begin(9600);
Motor_Sensor_Init();
Servo_Init();
Color_Init();
// while(1){
// Serialprint_gray_sensor_data_analogRead();
// }
}
void loop() {
// put your main code here, to run repeatedly:
Automatic_Tracking_analogRead();
if(Gray_Three == 1)
{
Tracking_Automatic_Tracking(1300);
Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);
return_color_card(); //小车走到色卡开始识别色卡
Tracking_Automatic_Tracking(3300);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(500);
return_color_ballon();
if(color_detection_card == color_detection_ballon){
Zha_Qi_Qiu(3);
finish = false;
finish_all = false;
}//小车走到第一个气球区域，并判断气球颜色。
if(finish_all){
Tracking_Automatic_Tracking(2800);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(500);
return_color_ballon();
if( (color_detection_card == color_detection_ballon) && finish ){
Zha_Qi_Qiu(3);
finish = false;
finish_all = false;
}//小车走到第二个气球区域，并判断气球颜色。
}
if(finish_all){
Tracking_Automatic_Tracking(3800);Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(500);
return_color_ballon();
if( (color_detection_card == color_detection_ballon) && finish ){
Zha_Qi_Qiu(3);
finish = false;
finish_all = false;
}//小车走到第三个气球区域，并判断气球颜色。
}
Car_Move(Stop,Car_speed_stop,Car_speed_stop);delay(3000);
}
}
void Zha_Qi_Qiu(int Numbers)
{
for(int i=0;i<Numbers;i++)
{
Servo_Move_Single(1,10,145,2);
Servo_Move_Single(1,145,10,3);
}
}
Arduino后驱扎气球小车C语言代码
浙公网安备 33010602011771号