u8 car_move_flag=0;
u8 uphill_flag=0;
u8 downhill_flag=0;
float set_car_speed_forward=0;
float car_speed_forward=0;
float spin_speed=0;
s32 car_distance=0;
s32 end_line_distance=0;
s32 unloading_distance=0;
s32 set_go_step_distance=0;
float track_data_min[8];
float track_data_max[8];
float track_data[8];
u8 black_line_status[8];
u8 black_horizontal_line_flag=0;
u8 lost_black_line_flag=0;
u8 black_line_valid=0;
u8 check_half_black_line_flag=0;
u8 half_black_line_flag=0;
float error;
//命令标志
u8 tracking_flag;//主循迹标志
u8 set_go_step_flag;//设定前进步数标志
u8 set_spin_step_flag;//设定旋转步数标志
u8 label;
u8 size;
u8 lighting=0;
u8 angle[3];
u32 pixels[2][6]={0};
materials_blob materials;
box box_mes[3];
//赛道参数初始化
void Track_Data_Init(void)
{
u8 i;
if(Read_AD_Max_Min()!=FR_OK)
{
for(i=0;i<8;i++)
{
track_data_min[i]=800;
track_data_max[i]=2200;
}
LCD_ShowString(0,0,(u8 *)"Read AD Max Min Falid");
R_LED(1);
delay_ms(500);
delay_ms(500);
LCD_Clear(BLACK);
}
if(Read_Pixels()!=FR_OK)
{
LCD_ShowString(0,0,(u8 *)"Read Pixels Falid");
R_LED(1);
delay_ms(500);
delay_ms(500);
LCD_Clear(BLACK);
}
set_car_speed_forward=400;
tracking_flag=1;
set_go_step_flag=0;
car_move_flag=1;
car_speed_forward=set_car_speed_forward;
box_mes[0].x1=loading_material_x1;
box_mes[0].y1=loading_material_y1;
box_mes[0].z1=loading_material_z1;
box_mes[1].x1=loading_material_x2;
box_mes[1].y1=loading_material_y2;
box_mes[1].z1=loading_material_z2;
box_mes[2].x1=loading_material_x3;
box_mes[2].y1=loading_material_y3;
box_mes[2].z1=loading_material_z3;
box_mes[0].x2=unloading_material_x1;
box_mes[0].y2=unloading_material_y1;
box_mes[0].z2=unloading_material_z1;
box_mes[1].x2=unloading_material_x2;
box_mes[1].y2=unloading_material_y2;
box_mes[1].z2=unloading_material_z2;
box_mes[2].x2=unloading_material_x3;
box_mes[2].y2=unloading_material_y3;
box_mes[2].z2=unloading_material_z3;
}
//判断黑线状态
void Judge_Black_Line_Status(void)
{
u8 i;
u8 numb=0;
u8 position[8];
for(i=0;i<8;i++)
{
if(track_data[i]>track_data_range*0.3) black_line_status[i]=0;
else black_line_status[i]=1;
}
for(i=0;i<8;i++)
{
if(black_line_status[i])
{
position[numb]=i;
numb++;
}
}
//黑线数超过三个舍弃
if(numb>3)
{
black_line_valid=0;
black_horizontal_line_flag=1;
}
else black_horizontal_line_flag=0;
//丢线
if(numb==0)
{
lost_black_line_flag=1;
}
else lost_black_line_flag=0;
//判断黑线位置是否相邻
if(numb==1 || \
(numb==2 && position[0]+1==position[1]) || \
(numb==3 && position[0]+1==position[1] && position[1]+1==position[2]))
{
black_line_valid=1;
}
else black_line_valid=0;
}
//归一化
void Track_Data_Normalization(void)
{
u8 i;
for(i=0;i<8;i++)
{
if(adc_data_fliter[i]>track_data_max[i]) track_data[i]=100;
else if(adc_data_fliter[i]<track_data_min[i]) track_data[i]=0;
else track_data[i]=(adc_data_fliter[i]-track_data_min[i])*track_data_range/(track_data_max[i]-track_data_min[i]);
}
}
//差比和
float Difference_Divide_Sum(void)
{
float difference,sum;
difference=4*track_data[0]+3*track_data[1]+2*track_data[2]+track_data[3] \
-track_data[4]-2*track_data[5]-3*track_data[6]-4*track_data[7];
sum=track_data[0]+track_data[1]+track_data[2]+track_data[3] \
+track_data[4]+track_data[5]+track_data[6]+track_data[7];
if(sum) difference=100*difference/sum;
else difference=0;
if(difference>100) difference=100;
else if(difference<-100) difference=-100;
return (difference);
}
//单边线求偏差
float Half_Black_Line_Error(void)
{
u8 i;
for(i=0;i<4;i++)
{
if(black_line_status[i]) break;
}
return ((4-i)-1)*9;
}
//确认误差
void Confirm_Error(void)
{
if(uphill_flag==1 || downhill_flag==1) error=Difference_Divide_Sum();//上下坡
else if(check_half_black_line_flag) error=-Half_Black_Line_Error();
else if(black_line_valid) error=Difference_Divide_Sum();
else if(lost_black_line_flag) error=0;
else error=0;
}
//中断赛道处理
void Track_Deal_Irq_20ms(void)
{
Track_Data_Normalization();
Judge_Black_Line_Status();
Confirm_Error();
}
void Distance_Check(void)
{
s32 distance;
distance=(s32)(CNT_10ms[0]+CNT_10ms[1]+CNT_10ms[2]+CNT_10ms[3]);
car_distance+=distance;
end_line_distance+=distance;
set_go_step_distance+=distance;
unloading_distance+=distance;
}
//设定车子行驶距离
void Set_Go_Step(float set_leg,float set_speed)
{
Set_Go_Step_Pid_Parameter_Init(set_leg*pulse_per_millimeter,set_speed);//复位距离pid
set_go_step_distance=0;
set_go_step_flag=1;
}
u8 Check_Go_Step(float range)
{
if(fabs(set_go_step_pid_point-set_go_step_distance)<range)
{
set_go_step_flag=0;
car_speed_forward=0;
return 1;
}
return 0;
}
//设定车子旋转角度
void Set_Spin_Step(float set_angle,float set_speed)
{
Set_Spin_Step_Pid_Parameter_Init(set_angle+yaw,set_speed);//复位距离pid
set_spin_step_flag=1;
}
u8 Check_Spin_Step(float range)
{
if(fabs(set_spin_step_pid_point-yaw)<range)
{
set_spin_step_flag=0;
return 1;
}
return 0;
}
//判断物料在哪个框
u8 Judge_Material_Numb(u8 _lable,u8 size)
{
u8 i;
for(i=0;i<3;i++)
{
if((box_mes[i].lable==_lable || (box_mes[i].lable==2 && _lable==6) || (box_mes[i].lable==6 && _lable==2)) && box_mes[i].size==size) break;
}
if(i==3) return 0;
else return (i+1);
}
//判断是否打光 白色物料需要打光
u8 Judge_Lighting(void)
{
u8 i,count=0;
u16 back_count=0;
for(i=0;i<judge_lighting_count;i++)
{
Clear_Openmv_Rxbuff();
Openmv_Send_Cmd("light");
back_count=0;
while(!Get_Openmv_Lighting(&lighting))
{
delay_ms(5);
back_count++;
if(back_count>300) return 0;
}
if(lighting) count++;
}
if(count>=judge_lighting_count/2) lighting=1;
else lighting=0;
return 1;
}
//定位物料
u8 Location_Material(u8 search_mode,u8 xy_mode)
{
u8 count=0;
u16 back_count=0;
char temp[20];
float x,y;
if(search_mode==5) sprintf-(temp,"search%d",1);
else sprintf(temp,"search%d",search_mode);
while(1)
{
Clear_Openmv_Rxbuff();
Openmv_Send_Cmd(temp);//发送搜索命令
back_count=0;
if(xy_mode)
{
x=materials.cx;
y=materials.cy;
}
else
{
x=materials.x;
y=materials.y;
}
while(!Get_Openmv_Materials_XY(&materials))//等待获取物料坐标
{
delay_ms(5);
back_count++;
if(back_count>300)
{
car_speed_forward=0;
return 0;
}
}
BACK_COLOR=BLACK;
POINT_COLOR=WHITE;
if(fabs(x)>65 || fabs(y)>60)
{
// LCD_ShowNumber(column(0),row(0),materials.x,3,0);
// LCD_ShowNumber(column(0),row(1),materials.y,3,0);
x=0;
y=0;
}
if(search_mode==1||search_mode==2)
{
car_speed_forward=(0-x)*1.0f;
Three_Dimensional_Move_Arm(arm_x-(0-y)*0.2f,arm_y,arm_z,0,90,200);
if(fabs(x)>8 || fabs(y)>8)
{
count=0;
}
else
{
if(count++>6) break;
}
}
else if(search_mode==3)
{
if(fabs(x)>8 || fabs(y)>8)
{
car_speed_forward=(0-x)*1.0f;
if(fabs(y)>5) Three_Dimensional_Move_Arm(arm_x-(0-y)*0.3f,arm_y,arm_z,0,90,200);
count=0;
}
else
{
if(count++>6) break;
}
}
else if(search_mode==5)
{
car_speed_forward=(0-x)*1.0f;
if(fabs(x)>8)
{
count=0;
}
else
{
if(count++>6) break;
}
}
else
{
Three_Dimensional_Move_Arm(arm_x+(0-x)*0.1f,arm_y+(0-y)*0.1f,arm_z,0,gripper_z_angle,200);
if(fabs(x)>8 || fabs(y)>8)
{
count=0;
}
else
{
if(count++>6) break;
}
}
}
car_speed_forward=0;
return 1;
}
//判断物料标签
//0 正方形
//1 六边形
//2 n
//3 三角形
//4 五角星
//5 圆
//6 z(n横放)
u8 Judge_Material_Label_Size(u8 mode)
{
u8 label_count[7]={0};
u8 i;
u8 max=0;
u16 back_count=0;
u32 a=0;
u8 label_;
for(i=0;i<judge_count;i++)
{
Clear_Openmv_Rxbuff();
Openmv_Send_Cmd("judge");
back_count=0;
while(!Get_Openmv_Materials_Labels_Size(&label,&materials.a))
{
delay_ms(5);
back_count++;
if(back_count>300) return 0;
}
label_count[label]++;
a+=materials.a;
}
for(i=0;i<7;i++)
{
if(label_count[i]>max)
{
label=i;
max=label_count[i];
}
}
a=a/judge_count;
if(mode<3 && mode>0)
{
if(label==6) label_=2;
else label_=label;
if(a>pixels[mode-1][label_]) size=1;
else size=0;
}
else size=0;
return 1;
}
u8 Judge_Material_Angle(u8 angle_mode)
{
u16 back_count=0;
char temp[20];
sprintf(temp,"angle%d",angle_mode);
Clear_Openmv_Rxbuff();
Openmv_Send_Cmd(temp);
while(!Get_Openmv_Angle(angle))
{
delay_ms(5);
back_count++;
if(back_count>300) return 0;
}
return 1;
}
//装载物料函数
void Loading_Material(void)
{
u8 count=0;
u8 scan_count=0;
u8 material_message_exit[7]={0};
u8 material_message_scanned[7]={0};
u8 res=0;
double speed_arm;
speed_arm=200;
Set_Go_Step(25,100);
while(!Check_Go_Step(100));
tracking_flag=0;
car_speed_forward=0;
Three_Dimensional_Move_Arm(arm_x,0,loading_area_z,0,90,150);
Three_Dimensional_Move_Arm(loading_area_x,arm_y,arm_z,0,90,150);
while(1)
{
//70%亮度灯
Grap_LED(test_light);
delay_ms(600);
//判断是否开全灯 白色要开全灯 其他颜色开低灯避免反光
res=Judge_Lighting();
if(lighting) Grap_LED(max_light);
else Grap_LED(little_light);
//粗定位
if(res) res=Location_Material(5,0);
//判断物料形状大小
if(res) res=Judge_Material_Label_Size(1);
//定位
res=Location_Material(1,0);
//播报形状大小
if(res) SERIAL_MP3_PLAY_W_INDEX(label*2+size+2);
//关灯
Grap_LED(0);
if((size==0 || material_message_scanned[label]) && !material_message_exit[label] && res)
{
if(label==2 || label==6)
{
material_message_exit[2]=1;
material_message_exit[6]=1;
}
else material_message_exit[label]=1;
box_mes[count].lable=label;
box_mes[count].lighting=lighting;
box_mes[count].size=size;
if(label==3)
{
res=Judge_Material_Angle(2);
if(angle[2]==0) Three_Dimensional_Move_Arm(arm_x+20,arm_y,arm_z-40,0,60,150);
else Three_Dimensional_Move_Arm(arm_x+20,arm_y,arm_z-40,0,90,150);
Three_Dimensional_Move_Arm(arm_x-20,arm_y,arm_z,0,gripper_z_angle,150);
}
else Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z-40,0,90,150);
delay_ms(200);
Control_Grap(0);
Three_Dimensional_Move_Arm(arm_x,arm_y,car_material_z,0,gripper_z_angle,speed_arm);
Three_Dimensional_Move_Arm(box_mes[count].x1,box_mes[count].y1,arm_z,0,0,speed_arm);
delay_ms(300);
Three_Dimensional_Move_Arm(arm_x,arm_y,box_mes[count].z1,0,0,180);
Control_Grap(1);
Control_Loading_Grap(count,0);
count++;
if(count==3) break;
}
//记录扫描过的物料
if(label==2 || label==6)
{
material_message_scanned[2]=1;
material_message_scanned[6]=1;
}
else material_message_scanned[label]=1;
scan_count++;
if(scan_count==6) break;
Three_Dimensional_Move_Arm(arm_x,arm_y,loading_area_z2,0,gripper_z_angle,speed_arm);
Three_Dimensional_Move_Arm(loading_area_x2,0,arm_z,0,90,speed_arm);
Grap_LED(test_light);
delay_ms(600);
res=Judge_Lighting();
if(lighting) Grap_LED(max_light);
else Grap_LED(little_light);
if(res) res=Location_Material(5,0);
if(res) res=Judge_Material_Label_Size(2);
if(res)
{
if(label==3) res=Location_Material(2,1);
else res=Location_Material(2,0);
}
if(res) SERIAL_MP3_PLAY_W_INDEX(label*2+size+2);
Grap_LED(0);
if((size==0 || material_message_scanned[label]) && !material_message_exit[label] && res)
{
if(label==2 || label==6)
{
material_message_exit[2]=1;
material_message_exit[6]=1;
}
else material_message_exit[label]=1;
box_mes[count].lable=label;
box_mes[count].lighting=lighting;
box_mes[count].size=size;
// BACK_COLOR=BLACK;
// POINT_COLOR=WHITE;
// LCD_ShowNumber(column(0),row(0),label,1,0);
Three_Dimensional_Move_Arm(arm_x-2,arm_y,arm_z-40,0,90,150);
delay_ms(200);
Control_Grap(0);
Three_Dimensional_Move_Arm(arm_x,arm_y,car_material_z,0,gripper_z_angle,speed_arm);
if(count==1) Three_Dimensional_Move_Arm(box_mes[count].x1,box_mes[count].y1,arm_z,0,2,speed_arm);
else Three_Dimensional_Move_Arm(box_mes[count].x1,box_mes[count].y1,arm_z,0,0,speed_arm);
delay_ms(300);
Three_Dimensional_Move_Arm(arm_x,arm_y,box_mes[count].z1,0,0,180);
Control_Grap(1);
Control_Loading_Grap(count,0);
count++;
if(count==3) break;
}
//记录扫描过的物料
if(label==2 || label==6)
{
material_message_scanned[2]=1;
material_message_scanned[6]=1;
}
else material_message_scanned[label]=1;
scan_count++;
if(scan_count==6) break;
Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z+50,0,gripper_z_angle,speed_arm);
Three_Dimensional_Move_Arm(loading_area_x,0,arm_z,0,90,speed_arm);
Three_Dimensional_Move_Arm(arm_x,arm_y,loading_area_z,0,90,speed_arm);
if(scan_count<4)
{
tracking_flag=1;
}
else
{
yaw=0;
set_spin_step_flag=1;
}
Set_Go_Step(35,100);
while(!Check_Go_Step(100));
set_car_speed_forward=100;
car_speed_forward=100;
while(Read_Right_Rled);
while(!Read_Right_Rled);
Set_Go_Step(35,100);
while(!Check_Go_Step(100));
check_half_black_line_flag=0;
tracking_flag=0;
set_spin_step_flag=0;
car_speed_forward=0;
}
Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z+100,0,0,speed_arm);
Three_Dimensional_Move_Arm(x_grap_0,y_grap_0,arm_z,0,90,speed_arm);
Three_Dimensional_Move_Arm(arm_x,arm_y,z_grap_0,0,90,speed_arm);
while(!Judge_Loading_Grap_Finish(2));
tracking_flag=1;
}
//卸载物料
void Unloading_Material(void)//装载物料函数
{
u8 numb;
u8 count=0;
u8 scan_count=0;
u8 res=0;
u8 wujiao_status[2];
u8 sanjiao_status[2];
float x;
double speed_arm;
u8 i;
if(box_mes[0].size==0 && box_mes[1].size==0 && box_mes[2].size==0)
{
i=3;
while(i--)
{
while(Read_Right_Rled);
car_distance=0;
while(!Read_Right_Rled || car_distance<45*pulse_per_millimeter);
}
scan_count=3;
}
speed_arm=200;
Set_Go_Step(25,50);
while(!Check_Go_Step(100));
tracking_flag=0;
car_speed_forward=0;
Three_Dimensional_Move_Arm(arm_x,0,unloading_area_z,0,90,150);
Three_Dimensional_Move_Arm(unloading_area_x,0,arm_z,0,90,150);
while(1)
{
Grap_LED(300);
res=Location_Material(3,0);
x=arm_x;
if(res) Judge_Material_Label_Size(0);
if(label==4)
{
if(res)
{
res=Judge_Material_Angle(1);
wujiao_status[1]=angle[1];
// BACK_COLOR=BLACK;
// POINT_COLOR=WHITE;
// LCD_ShowNumber(column(0),row(2),angle[1],1,0);
}
}
if(res) Judge_Material_Label_Size(0);
if(label==3)
{
if(res)
{
res=Judge_Material_Angle(2);
sanjiao_status[1]=angle[1];
// BACK_COLOR=BLACK;
// POINT_COLOR=WHITE;
// LCD_ShowNumber(column(0),row(2),angle[1],1,0);
}
}
Grap_LED(0);
// BACK_COLOR=BLACK;
// POINT_COLOR=WHITE;
// LCD_ShowNumber(column(0),row(0),label,1,0);
if(res)
{
if(scan_count<3) numb=Judge_Material_Numb(label,1);
else numb=Judge_Material_Numb(label,0);
}
if(numb && res)
{
Three_Dimensional_Move_Arm(arm_x,0,box_mes[numb-1].z2,0,0,speed_arm);
Three_Dimensional_Move_Arm(box_mes[numb-1].x2,box_mes[numb-1].y2,arm_z,0,0,speed_arm);
if(box_mes[numb-1].lighting) Grap_LED(max_light);
else Grap_LED(little_light);
if(numb==2) gripper_z_angle=2;
Location_Material(4,0);
if(label==4)
{
Judge_Material_Angle(1);
wujiao_status[0]=angle[0];
}
else if(label==3)
{
Judge_Material_Angle(2);
if(angle[2]==1)
{
if(angle[0]==0) sanjiao_status[0]=0;
else sanjiao_status[0]=1;
}
else
{
if(angle[1]==0)
{
if(angle[0]==0) sanjiao_status[0]=2;
else sanjiao_status[0]=3;
}
else
{
if(angle[0]==0) sanjiao_status[0]=4;
else sanjiao_status[0]=5;
}
}
}
Grap_LED(0);
if(box_mes[numb-1].lable==3)
{
if(sanjiao_status[1]==0)
{
if(sanjiao_status[0]==0) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,-60,speed_arm);
else if(sanjiao_status[0]==1) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,60,speed_arm);
else if(sanjiao_status[0]==2) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==3) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==4) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,60,speed_arm);
else if(sanjiao_status[0]==5) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,60,speed_arm);
}
else
{
if(sanjiao_status[0]==0) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==1) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,-60,speed_arm);
else if(sanjiao_status[0]==2) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,60,speed_arm);
else if(sanjiao_status[0]==3) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,60,speed_arm);
else if(sanjiao_status[0]==4) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==5) Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,60,speed_arm);
}
}
if(box_mes[numb-1].lable==6)
{
Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z,0,-90,speed_arm);
delay_ms(200);
}
if(numb==3) Three_Dimensional_Move_Arm(arm_x,arm_y-1,box_mes[numb-1].z1,0,gripper_z_angle,180);
else Three_Dimensional_Move_Arm(arm_x,arm_y,box_mes[numb-1].z1,0,gripper_z_angle,180);
delay_ms(200);
Control_Loading_Grap(numb-1,1);
Control_Grap(0);
delay_ms(500);
// Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z+30,0,gripper_z_angle,speed_arm);
Three_Dimensional_Move_Arm(arm_x,arm_y,car_material_z+30,0,gripper_z_angle,speed_arm);
if(label==3)
{
if(sanjiao_status[1]==0)
{
if(sanjiao_status[0]==0) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,120,speed_arm);
else if(sanjiao_status[0]==1) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,60,speed_arm);
else if(sanjiao_status[0]==2) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,60,speed_arm);
else if(sanjiao_status[0]==3) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,120,speed_arm);
else if(sanjiao_status[0]==4) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==5) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,-60,speed_arm);
}
else
{
if(sanjiao_status[0]==0) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==1) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,120,speed_arm);
else if(sanjiao_status[0]==2) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,-60,speed_arm);
else if(sanjiao_status[0]==3) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,0,speed_arm);
else if(sanjiao_status[0]==4) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,120,speed_arm);
else if(sanjiao_status[0]==5) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,-60,speed_arm);
}
}
else if(label==4)
{
if(wujiao_status[0]==wujiao_status[1]) Three_Dimensional_Move_Arm(x-95,0,arm_z,0,90+11,speed_arm);
else Three_Dimensional_Move_Arm(x-95,0,arm_z,0,90-11,speed_arm);
}
else Three_Dimensional_Move_Arm(x-95,0,arm_z,0,90,speed_arm);
delay_ms(300);
Three_Dimensional_Move_Arm(arm_x,arm_y,arm_z-100,0,gripper_z_angle,150);
Control_Grap(1);
count++;
}
scan_count++;
if(count==3 || scan_count==6) break;
Three_Dimensional_Move_Arm(unloading_area_x,0,arm_z,0,90,speed_arm);
Three_Dimensional_Move_Arm(arm_x,0,unloading_area_z,0,90,speed_arm);
tracking_flag=1;
Set_Go_Step(35,100);
while(!Check_Go_Step(100));
set_car_speed_forward=100;
while(Read_Right_Rled);
while(!Read_Right_Rled);
Set_Go_Step(25,100);
while(!Check_Go_Step(100));
tracking_flag=0;
car_speed_forward=0;
}
Three_Dimensional_Move_Arm(x_grap_0,y_grap_0,arm_z,0,90,speed_arm);
Three_Dimensional_Move_Arm(arm_x,arm_y,z_grap_0,0,90,speed_arm);
tracking_flag=1;
}
//避障
void Collision_Avoidance(void)
{
Set_Go_Step(300,300);
while(!Check_Go_Step(80));
tracking_flag=0;
car_speed_forward=0;
// delay_ms(400);
Set_Spin_Step(90,800);
while(!Check_Spin_Step(5));
set_spin_step_flag=1;
Set_Go_Step(500,800);
while(!Check_Go_Step(80));
// delay_ms(400);
Set_Spin_Step(-90,800);
while(!Check_Spin_Step(5));
set_spin_step_flag=1;
Set_Go_Step(1400,800);
while(!Check_Go_Step(80));
// delay_ms(400);
Set_Spin_Step(-90,800);
while(!Check_Spin_Step(5));
set_spin_step_flag=1;
car_speed_forward=200;
car_distance=0;
while(!black_horizontal_line_flag && car_distance<600*pulse_per_millimeter);
Set_Go_Step(200,300);
while(!Check_Go_Step(80));
delay_ms(200);
Set_Spin_Step(90,800);
while(!Check_Spin_Step(5));
tracking_flag=1;
}
void Track_Element_Init(void)
{
R_LED(1);
car_move_flag=0;
while(!key_sign[2] && !SW4);
car_move_flag=1;
key_sign[2]=0;
R_LED(0);
car_distance=0;
yaw=0;
}
//循环赛道处理
void Track_Deal_Loop(void)
{
car_move_flag=0;
key_sign[2]=0;
while(!key_sign[2]);
key_sign[2]=0;
Direction_Pid_Parameter_Init(0,9,2);
Track_Element_Init();
set_car_speed_forward=400;
//变形
while(1)
{
if(car_distance>300*pulse_per_millimeter)
{
Three_Dimensional_Move_Arm(x_grap_0,y_grap_0,arm_z,0,90,200);
Three_Dimensional_Move_Arm(arm_x,arm_y,z_grap_0,0,90,200);
break;
}
}
//转第一个弯
Track_Element_Init();
while(1)
{
if(yaw>45 && black_horizontal_line_flag)
{
break;
}
}
//上坡
Direction_Pid_Parameter_Init(0,1,1);
Track_Element_Init();
SERIAL_MP3_PLAY_R_INDEX(1);
while(1)
{
//角度检测
if(roll<-11 && uphill_flag==0)
{
uphill_flag=1;
}
else if(roll>-10 && uphill_flag==1) uphill_flag=2;
if(car_distance>300*pulse_per_millimeter && uphill_flag==2)
{
break;
}
if(uphill_flag==1)
{
R_LED(1);
G_LED(1);
B_LED(1);
}
}
//上料
check_half_black_line_flag=1;
set_car_speed_forward=100;
Direction_Pid_Parameter_Init(0,9,2);
Track_Element_Init();
SERIAL_MP3_PLAY_R_INDEX(2);
while(1)
{
if(Read_Right_Rled && car_distance>200*pulse_per_millimeter)
{
Loading_Material();
break;
}
if(half_black_line_flag)
{
R_LED(1);
G_LED(0);
B_LED(1);
}
}
//下坡
set_car_speed_forward=400;
Direction_Pid_Parameter_Init(0,2,0);
Track_Element_Init();
while(1)
{
//角度检测
if(roll>8 && downhill_flag==0)
{
downhill_flag=1;
}
else if(roll<7 && downhill_flag==1)
{
downhill_flag=2;
car_distance=0;
}
if(car_distance>200*pulse_per_millimeter && downhill_flag==2)
{
break;
}
if(downhill_flag==1)
{
R_LED(1);
G_LED(1);
B_LED(1);
}
else if(half_black_line_flag)
{
R_LED(1);
G_LED(0);
B_LED(1);
}
}
//转弯
Direction_Pid_Parameter_Init(0,9,2);
Track_Element_Init();
while(1)
{
if(car_distance>300*pulse_per_millimeter && yaw>45 && black_horizontal_line_flag)
{
break;
}
}
//障碍
Track_Element_Init();
SERIAL_MP3_PLAY_R_INDEX(3);
Collision_Avoidance();
while(1)
{
if(yaw>70)
{
if(car_distance>100*pulse_per_millimeter) break;
}
else car_distance=0;
}
//下料
set_car_speed_forward=200;
check_half_black_line_flag=1;
Track_Element_Init();
SERIAL_MP3_PLAY_R_INDEX(4);
while(1)
{
if(Read_Right_Rled)
{
unloading_distance=0;
SERIAL_MP3_PLAY_R_INDEX(5);
Unloading_Material();
break;
}
if(half_black_line_flag)
{
R_LED(1);
G_LED(0);
B_LED(1);
}
}
//走完下料区
set_car_speed_forward=400;
Track_Element_Init();
while(1)
{
if(car_distance>600*pulse_per_millimeter || yaw>15 || unloading_distance>900*pulse_per_millimeter)
{
break;
}
if(half_black_line_flag)
{
R_LED(1);
G_LED(0);
B_LED(1);
}
}
//终点线检测
check_half_black_line_flag=0;
Track_Element_Init();
while(1)
{
if(lost_black_line_flag)
{
if(end_line_distance>400*pulse_per_millimeter)
{
car_move_flag=0;
SERIAL_MP3_PLAY_R_INDEX(6);
}
}
else end_line_distance=0;
}
while(1);
}
void Car_Move_Control(void)
{
float set_forward_speed=0;//前进速度
float set_spin_speed=0;//旋转速度
float car_speed_max;//小车最大速度
if(car_move_flag)//如果小车前进标记等于1
{
if(set_go_step_flag)//如果小车走固定的距离
{
//设置距离PID
Set_Go_Step_Pid_Calculate(set_go_step_distance);
//得到小车前进速度
car_speed_forward=set_go_step_pid_uk;
}
//如果是循迹标记
if(tracking_flag)
{
//如果不是固定步长
if(!set_go_step_flag)
{
//小车速度，最大速度
car_speed_max=set_car_speed_forward-S2(error)*0.02f;
if(car_speed_max<50) car_speed_max=50;
//如果小车速度没有达到最大值，那么就不断 加8
if(car_speed_forward<car_speed_max) car_speed_forward+=8;
else car_speed_forward=car_speed_max;
//否则就是最大速度
}
//方向PID计算用z轴的角速度做误差
Direction_Pid_Calculate(error,-z_angular_speed);
//设置旋转角度
set_spin_speed=-direction_pid_uk;
}
//如果是设置的旋转角度
if(set_spin_step_flag)
{
//将yaw角度做旋转反馈
Set_Spin_Step_Pid_Calculate(yaw);
//将误差反馈回来
set_spin_speed=-set_spin_step_pid_uk;
}
set_forward_speed=car_speed_forward;
if((set_forward_speed>0 && tracking_flag) || !tracking_flag)
Mecanum_Wheel_Motor_Calculate(set_forward_speed,0,set_spin_speed);
//只有前进时循迹
else Mecanum_Wheel_Motor_Calculate(set_forward_speed,0,0);
}
//如果小车不前进，那么就停止
else
{
Mecanum_Wheel_Motor_Calculate(0,0,0);
}
}
循迹csdn
原创
©著作权归作者所有：来自51CTO博客作者XGLIYOUQUAN的原创作品，请联系作者获取转载授权，否则将追究法律责任
提问和评论都可以，用心的回复会被更多人看到
评论
发布评论
相关文章
-
PythonPython
-
Python之FastAPI构建Python Web
FastAPI 构建高效的 Python Web
ico Python 服务器 -
灰度。。循迹头
dddddddddddddd
单片机 stm32 -
240...循迹
5555555555555555555
学习 51单片机
