2018年工程训练综合能力竞赛作品相关

小车前期通过ug建模，3d模型图如下

通过激光切割，3d打印等工艺做出实物，如下图

小车的任务是通过扫码抓取上料区指定的物料，并放入下料区对应条形码的框中，难点在于如何保证抓放物料的精度。
小车主控为stm32f429，使用现成的扫码模块扫码，通过串口与单片机通信，小车对四个电机进行了闭环控制，定位精准，小车还拥有一个独特的机械臂控制算法，可以非常方便的做到机械臂调试，运动流畅。如下图所示


机械臂控制算法如下

/************************普通运算*************************/
#define adjust_half_16(a) (uint16_t)(a+0.5)//16位四舍五入
#define adjust_half_8(a) (uint8_t)(a+0.5)//8位四舍五入

//平方
#define S2(x) ((x)*(x))
//余弦定理
#define cosine(l1,l2,l3) acos((S2(l1)+S2(l2)-S2(l3))/(2*(l1)*(l2)))
//正弦定理 
#define Sine(a,la,lb) asin(sin(a)/(la)*(lb))
//弧度制转换 弧度到角度
#define R_A 57.2958
//角度到弧度
#define A_R 0.01745329
//求两点长度
#define len(x1,y1,x2,y2) sqrt(S2((x1)-(x2))+S2((y1)-(y2)))
/******机械臂参数******/
//第一个舵机最大转速 弧度制
#define max_speed_0 4
#define max_speed_3 8.0

#define max_speed_5 1496.0
//各个机械臂长度 单位mm
#define L1 241.00
#define L2 157.9381
//各个机械臂在特定位置的占空比
#define flat_0 1265//平齐
#define vertical_0 235//垂直
//#define upright_1 959//1011//934//947//竖直
#define flat_1 1661//1680//1878//1842//1841//水平
#define flat_2 1796.0//平齐
#define flat_3 643.0
#define flat_4 282.0
#define open_5 742.0//张开
#define close_5 330.0//闭合
#define check_5 250//检测
#define lay_5 250//放置
#define back_5 490//退出
//占空比方向 正值为增长旋转方向相同
#define dir0 (-1)
#define dir1 (1)
#define dir2 (-1)
#define dir3 (-1)
#define dir4 (-1)
//初始位置
#define x_grap_0 -10.0
#define y_grap_0 0.0
#define z_grap_0 140.0
#define z_angle_0 90.0
//输出最大值 最小值
#define arm_max 2000.0
#define arm_min 0.0
//弧度制到占空比
#define c_value_0 655.7184
#define c_value_1 450.0902//415.9896//589.0//429.7184//578.0508
#define c_value_2 576.1409
#define c_value_3 541.1268
#define c_value_4 558.9521
//各个变量参数
u16 arm_target_duty[7];//7个舵机目标值
double arm_target_angle[7]={0};//7个舵机目标值

double arm_x=0;
double arm_y=120; 
double arm_z=120;
double gripper_z_angle=0;

double ave_x,ave_y,ave_z,ave_z_angle;
u8 gripper_status;
double arm_target_x,arm_target_y,arm_target_z,arm_target_z_angle,arm_target_gripper;
u16 arm_20ms_count=0;
//标志
u8 arm_finish_flag=0;

void Spatial_Coordinates_Calculate(double x,double y,double z,double z_angle)//空间坐标
{
	double lxy;
	double lxy_z;
	
	if(y>0.01) arm_target_angle[0]=atan(x/y);//获取偏移角
	else if(y>=0) arm_target_angle[0]=-1.5708;
	else arm_target_angle[0]=0;
	
	arm_target_duty[0]=adjust_half_16(flat_0-dir0*arm_target_angle[0]*c_value_0);
	arm_target_duty[4]=adjust_half_16(flat_4+dir4*(arm_target_angle[0]+z_angle*A_R)*c_value_4);
	
	lxy=len(x,y,0,0);
  if((y==0&&x>0)||(y<0&&x==0)) lxy=-lxy;
	
	lxy_z=len(lxy,z,0,0);
	
	arm_target_angle[2]=cosine(L1,L2,lxy_z);
  arm_target_duty[2]=adjust_half_16(dir2*c_value_2*(3.1416-arm_target_angle[2])+flat_2);
	if(lxy>0) arm_target_angle[1]=Sine(arm_target_angle[2],lxy_z,L2)+atan(z/lxy);
	else if(lxy==0) arm_target_angle[1]=Sine(arm_target_angle[2],lxy_z,L2)+1.5708;
	else if(lxy<0) arm_target_angle[1]=Sine(arm_target_angle[2],lxy_z,L2)+3.1416+atan(z/lxy);
	arm_target_duty[1]=adjust_half_16(flat_1-dir1*c_value_1*arm_target_angle[1]);
	arm_target_angle[3]=2.0*3.1416-arm_target_angle[2]-arm_target_angle[1];
	arm_target_duty[3]=adjust_half_16(dir3*c_value_3*(3.1416-arm_target_angle[3])+flat_3);
}
void Mechanical_Arm_Control_Irq_20ms(void)//机械臂控制
{
	if(arm_20ms_count>0)
	{
		arm_20ms_count--;
		if(arm_20ms_count==0)
		{
			arm_x=arm_target_x;
	    arm_y=arm_target_y;
	    arm_z=arm_target_z;
			gripper_z_angle=arm_target_z_angle;
		}
		else 
		{
	    arm_x+=ave_x;
		  arm_y+=ave_y;
		  arm_z+=ave_z;
			gripper_z_angle+=ave_z_angle;
		}
		Spatial_Coordinates_Calculate(arm_x,arm_y,arm_z,gripper_z_angle); 
		Mechanical_Arm_Duty_Out();
	}
	else 
	{
		arm_finish_flag=1;
		TIM_Cmd(TIM7, DISABLE);
	}
}
void Three_Dimensional_Move_Arm(double length_x,double length_y,double length_z,\
	u8 gripper_status,double z_angle,double set_speed)//三维移动
{
	double angle_x,lxyz,t1,t2;//偏移角
	if(length_y>0.01) angle_x=atan(length_x/length_y);//获取偏移角
	else if(length_y>=0) angle_x=-1.5708;
	else angle_x=0;
	
	t1=fabs(angle_x-arm_target_angle[0])/max_speed_0;
	lxyz=sqrt(S2((length_x)-(arm_x))+S2((length_y)-(arm_y))+S2((length_z)-(arm_z)));
	t2=lxyz/set_speed;
	if(t1>t2) arm_20ms_count=adjust_half_16(t1*50)+1;
	else arm_20ms_count=adjust_half_16(t2*50)+1;//速度修正
	
	ave_x=(length_x-arm_x)/arm_20ms_count;
	ave_y=(length_y-arm_y)/arm_20ms_count;
	ave_z=(length_z-arm_z)/arm_20ms_count;
	ave_z_angle=(z_angle-gripper_z_angle)/arm_20ms_count;
	arm_target_x=length_x;
	arm_target_y=length_y;
	arm_target_z=length_z;
	arm_target_z_angle=z_angle;
	arm_target_gripper=gripper_status;
	arm_finish_flag=0;
	TIM_Cmd(TIM7, ENABLE);
	while(!arm_finish_flag);
}
void Tim7_Init_20ms(void)//20ms定时	
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); 
	
	TIM_TimeBaseStructure.TIM_Period=20000-1;
	TIM_TimeBaseStructure.TIM_Prescaler=84-1;
	TIM_TimeBaseInit(TIM7,&TIM_TimeBaseStructure);
	
	TIM_Cmd(TIM7, DISABLE);
	TIM_ITConfig(TIM7,TIM_IT_Update,ENABLE);
	TIM_ClearFlag(TIM7,TIM_FLAG_Update);
}	

void Arm_Init(void)
{
	Spatial_Coordinates_Calculate(0,120,120,0);
	Tim7_Init_20ms();
}