机械臂.zip

#include "control.h" #include "oled.h" #include "sys.h" /************************************************************************** **************************************************************************/ int Balance_Pwm,Velocity_Pwm,Turn_Pwm; u8 Flag_Target; u32 Flash_R_Count; u8 color; u8 init_flag=0; u8 head_flag=0;//车头寻迹模块灯全灭 u8 TAIL_flag=0;//尾部寻迹模块灯全灭 u8 end_ok;//完成抓取或者放置，并且回到初始位置 u8 H_xun,T_xun;//头寻迹和尾巴寻迹 u8 T_anx; int Voltage_Temp,Voltage_Count,Voltage_All; float Last_TargetX,Last_TargetY,Last_TargetZ; float temp_radian[5]; float RecyclableTarget[]={909,754,1016,1109}; float KitchenTarget[]={789,780,1016,1109}; float OtherTarget[]={672,751,1036,1091}; float HarmfulTarget[]={1010,595,855,1110}; u8 count_time; u8 bi_flag=0; u8 bi_step=0; #define l0 0.105f #define l1 0.10f #define l2 0.14f #define L2 0.105f #define L3 0.10f #define L4 0.14f //#define Grasp 750 //夹紧的舵机5数值 //#define Unstuck 250//松开的舵机5数值 //#define l1 0.078f //#define l2 0.055f #define Ratio 5.06f float temp_alpha=-2.355; u8 step=0; int count=0; /************************************************************************** 函数功能：计算发送的数据校验位 入口参数： 返回 值：检验位 **************************************************************************/ char Check_Sum( char *Count_Number,unsigned char size) { char check_sum=0,k; for(k=0;k< size;k++) { check_sum=check_sum^Count_Number[k]; } return check_sum; } /************************************************************************** 函数功能：float数据类型的绝对值 入口参数：float num 返回 值：绝对值 **************************************************************************/ float abss(float num) { if(num>=0) return num; else return -num; } /************************************************************************** 函数功能：用正解验算逆解的正确性 入口参数：float x,float y,float z:x,y,z坐标 float theta1,theta3,theta2,theta4：各个关节的旋转角度(注意：是弧度值不是角度值) 返回 值：　１：正确 ０：错误 **************************************************************************/ int is_right_solution(float x,float y,float z,float theta1,float theta2,float theta3,float theta4) { float x1,y1,z1,d; d=l0*cos(theta2)+l1*cos(theta2+theta3)+l2*cos(theta2+theta3+theta4); z1=l0*sin(theta2)+l1*sin(theta2+theta3)+l2*sin(theta2+theta3+theta4); x1=d*sin(theta1); y1=d*cos(theta1); if(abss(x1-x)>1e-2||abss(y1-y)>1e-2||abss(z1-z)>1e-2) return -1; return 1; } /************************************************************************** 函数功能：判断角度合法性 入口参数：float theta:角度 u8 joint_num：关节序号 theta1(原beta)取值范围：-0.587PI到0.359PI theta2(原theta1):(5/180)*PI--(166/180)*PI theta3(原theta2):-PI/2----(77/180)*PI) theta4(原theta3):-7PI/18-----PI/2 返回值：true false **************************************************************************/ int joint_angle_legal(float theta,u8 joint_num) { int temp; switch(joint_num) { case 1:if(theta>=(-0.5*PI)&&theta<=0.5*PI) temp= 1;else temp= -1;break; case 2:if(theta>=((0)*PI)&&theta<=PI) temp= 2;else temp= -2; break; case 3:if(theta>=(-PI/2)&&theta<=(0.5)*PI) temp= 3;else temp= -3;break; case 4:if(theta>=(-0.5*PI)&&theta<=PI/2) temp= 1;else temp= -1;break; } return temp; } /************************************************************************** 函数功能：产生a角的一个随机角度数(非弧度) 入口参数： theta1(原beta)取值范围：-0.587PI到0.359PI theta2(原theta1):(5/180)*PI--(166/180)*PI theta3(原theta2):-PI/2----PI/2 theta4(原theta3):-PI/2-----PI/2 a:(根据实际情况得出取值范围)：-3PI/4---PI/2 (-135---90度) **************************************************************************/ u32 random1() { u32 temp; //srand(time(0)); temp=rand()%226-135; return temp; } float angle_to_radian(u32 angle)//角度转弧度 { float temp; temp=angle*PI/180; return temp; } float radian_to_angle(float radian) { float angle; angle=radian*180/PI; return angle; } /************************************************************************** 函数功能：数学模型求逆解(优化算法) 入口参数：float x,float y,float z:x,y,z坐标 theta1 theta2: theta3: theta4: a:(根据实际情况得出取值范围) **************************************************************************/ int inserve_kinematics(float wx,float wy,float wz) { float distance,theta1,theta2,theta3,theta4,d; float b,z,cos_theta3,sin_theta3; float k1,k2; distance=sqrt(wx*wx+wy*wy+wz*wz); d=sqrt(wx*wx+wy*wy); if(distance>(L2+L3+L4)) { printf("too long\n"); return -1; } if((wx==0)&&(wy==0)) { theta1=0; } else { theta1=asin(wx/d); if(joint_angle_legal(theta1,1)<0) { return -2; } } temp_alpha=temp_alpha+0.02; if(temp_alpha>=1.57) { // printf("error\n"); start_flag=0; temp_alpha=-2.355; return -1; } temp_radian[1]=temp_alpha; // printf("temp:%f\n",temp_alpha); b=d-L4*cos(temp_alpha); z=wz-L4*sin(temp_alpha); cos_theta3=(z*z+b*b-L2*L2-L3*L3)/(2*L2*L3); if(abss(cos_theta3)>1) return -3; sin_theta3=sqrt(1-cos_theta3*cos_theta3); theta3=atan2(-sin_theta3,cos_theta3); if(joint_angle_legal(theta3,3)<0) { return -4 ; } temp_radian[3]=theta3; k1=L2+L3*cos(theta3); k2=L3*sin(theta3); theta2=atan2(z,b)-atan2(k2,k1); theta4=temp_alpha-theta3-theta2; temp_radian[2]=theta2; temp_radian[4]=theta4; if((joint_angle_legal(theta2,2)<0)||(joint_angle_legal(theta4,4)<0)) { return -2 ; } if(is_right_solution(TargetX,TargetY,TargetZ,theta1,theta2,theta3,theta4)<0) { return -6; } Target1 = 750-(radian_to_angle(theta1))*5.0; //作用到输出 Target2=-5.56*radian_to_angle(theta2)+1250; Target3 = 750+(radian_to_angle(theta3))*6.0; Target4 = 750+(radian_to_angle(theta4))*6.0; Target_Alpha=temp_alpha; // printf("1:%f.4 2:%.4f 3:%.4f 4:%.4f a:%.4f\n",radian_to_angle(theta1),radian_to_angle(theta2),radian_to_angle(theta3),radian_to_angle(theta4),radian_to_angle(Target_Alpha)); // printf("t1:%.4f t2:%.4f t3:%.4f t4:%.4f\n",Target1,Target2,Target3,Target4); return 4; } /************************************************************************** 函数功能:ABC模式下计算末端执行器的位置坐标 float TargetX=0.15,TargetY=-0.15,TargetZ=0.1,Target_Beta=0,Target_Alpha=0,Target_Gamma=0; float Target1=660,Target2=730,Target3=750,Target4=730,Target5=450,Target6=800; //电机目标值 Target1 = 660-(Beta)*Ratio; //作用到输出 Target2 = 730-(theta1-90)*Ratio; Target3 = 770+(theta2)*Ratio; Target4 = 700+(theta3)*Ratio; Y4 = 5.615233763116244*x + 714.1838206873543 y3 = 5.447043157528549*x + 766.5773232333704 y2 = -5.274503075479348*x + 1182.9972709649717 **************************************************************************/ void count_xyz() { float beta,theta1,theta3,theta2,d,z; beta=((750-Target1)/Ratio)*PI/180; //theta1=((750-Target2)/Ratio+90)*PI/180; theta1=((Target2-1250)/(-5.56))*PI/180; theta2=((Target3-750)/5.56)*PI/180; theta3=((Target4-750)/5.56)*PI/180; d=l0*cos(theta1)+l1*cos(theta1+theta2)+l2*cos(theta1+theta2+theta3); z=l0*sin(theta1)+l1*sin(theta1+theta2)+l2*sin(theta1+theta2+theta3); TargetZ=z; TargetX=d*sin(beta); TargetY=d*cos(beta); Target_Alpha=(theta1+theta2+theta3); } /************************************************************************** 函数功能：判断前末端执行器状态