机械臂抓取过程

这份代码基于stm32芯片结合平衡小车之家的库函数开发的舵机机械臂，抓取过程的。

#include “control.h“	
#include “stm32f10x.h“
#include “sys.h“
  /**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
int Balance_PwmVelocity_PwmTurn_Pwm;
u8 Flag_Target;
u32 Flash_R_Count;
int Voltage_TempVoltage_CountVoltage_All;
float Last_TargetXLast_TargetY;
#define l0 0.1635f
#define l1 0.151f
#define l2 0.076f  
#define Ratio 5.111f

/**************************************************************************
函数功能：数学模型
入口参数：末端执行器位姿态
返回  值：无
**************************************************************************/
u8 Kinematic_Analysis（float xfloat yfloat Betafloat Alphafloat Gamma）
{  
	      float mnkabctheta1theta2theta3s1ps2;
				m=l2*cos（Alpha）-x;   //中间变量
				n=l2*sin（Alpha）-y;   //中间变量
	      //k=（l0*l0-l1*l1-m*m-n*n）/2/l1;     //过程系数
	      k=（l1*l1-l0*l0-m*m-n*n）/2/l0;//中间变量
	      a=m*m+n*n;             //解一元二次方程
	      b=-2*n*k;
	      c=k*k-m*m;
	   
	      if（b*b-4*a*c<=0） 
        { 
				TargetX=Last_TargetX;		
			  TargetY=Last_TargetY;		
				return 0; //防止出现非实数解
				}
	
	      theta1=（-b+sqrt（b*b-4*a*c））/2/a;  //得到一元二次方程的解，只取其中一个，另外一个解是（-b+sqrt（b*b-4*a*c））/2/a
	      theta1=asin（theta1）*180/PI;       //弧度换成角度
				Theta1_Temp = theta1;
				
	      if（theta1>180）theta1=180;           //控制舵机的最大角度±90°
				if（theta1<-180）theta1=-180;
				
			   //k=（l1*l1-l0*l0-m*m-n*n）/2/l0;//中间变量
			  k=（l0*l0-l1*l1-m*m-n*n）/2/l1;
				a=m*m+n*n;                        //解一元二次方程
				b=-2*n*k;
				c=k*k-m*m;
	
	      if（b*b-4*a*c<=0）  
				{	
				TargetX=Last_TargetX;		
			  TargetY=Last_TargetY;
				return 0;         //防止出现非实数解
				}
	      s1ps2=（-b-sqrt（b*b-4*a*c））/2/a;      //得到一元二次方程的解，只取其中一个，另外一个解是（-b+sqrt（b*b-4*a*c））/2/a
	      s1ps2=asin（s1ps2）*180/PI;            //弧度换成角度
	
				if（s1ps2>180） theta2=180;    
				if（s1ps2<-180）theta2=-180;  
				
	      theta2=s1ps2-theta1;
				Theta2_Temp = theta2;
			  if（theta2>180）theta2=180;      //控制舵机的最大角度±90°
				if（theta2<-180）theta2=-180;    //控制舵机的最大角度±90°
	
	      theta3=Alpha*180/PI-theta1-theta2;   //求关节3角度
				Theta3_Temp = theta3;
				if（theta3>180）theta3=180;
				if（theta3<-180）theta3=-180;	    //控制舵机的最大角度±90°
				
				
				
				
				
       // Target1   = 750-（Beta）*Ratio;   //作用到输出  
        Target1   = 590-（theta1-90）*Ratio;  //400
	      Target2   = 780-（theta2）*Ratio;   //730
	      Target3   = 550-（theta3）*Ratio;  //增大值往下  //680
			//	Target4   = 800-（Gamma）*Ratio;   //6自由度额外增加的自由度  
			

				
				Last_TargetX=TargetX;
				Last_TargetY=TargetY;
				return 0;
}

/**************************************************************************
函数功能：定时中断函数 所有的控制代码都在这里面		 
**************************************************************************/
int TIM3_IRQHandler（void）
{    
	u8 temp;
	if （TIM_GetITStatus（TIM3 TIM_IT_Update） != R

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        399  2011-05-09 18:17  机器人机械臂最终版\keilkill.bat

     文件      13368  2018-08-10 14:47  机器人机械臂最终版\MiniBalance\CONTROL\control.c

     文件       1083  2017-08-25 07:45  机器人机械臂最终版\MiniBalance\CONTROL\control.h

     文件       2891  2015-07-09 14:05  机器人机械臂最终版\MiniBalance\DataScope_DP\DataScope_DP.C

     文件        586  2015-07-09 14:05  机器人机械臂最终版\MiniBalance\DataScope_DP\DataScope_DP.h

     文件       6276  2018-07-31 00:13  机器人机械臂最终版\MiniBalance\show\show.c

     文件        354  2016-01-06 14:35  机器人机械臂最终版\MiniBalance\show\show.h

     文件      17273  2010-06-07 10:25  机器人机械臂最终版\MiniBalance_COER\core_cm3.c

     文件      85714  2011-02-09 14:59  机器人机械臂最终版\MiniBalance_COER\core_cm3.h

     文件      12765  2011-03-10 10:52  机器人机械臂最终版\MiniBalance_COER\startup_stm32f10x_md.s

     文件       2935  2017-08-25 12:12  机器人机械臂最终版\MiniBalance_HARDWARE\ADC\adc.c

     文件        482  2017-07-12 22:49  机器人机械臂最终版\MiniBalance_HARDWARE\ADC\adc.h

     文件       2887  2015-06-07 00:48  机器人机械臂最终版\MiniBalance_HARDWARE\DataScope_DP\DataScope_DP.C

     文件        581  2015-05-04 12:37  机器人机械臂最终版\MiniBalance_HARDWARE\DataScope_DP\DataScope_DP.h

     文件      11040  2016-09-27 07:59  机器人机械臂最终版\MiniBalance_HARDWARE\IIC\IOI2C.c

     文件       3118  2016-03-05 15:33  机器人机械臂最终版\MiniBalance_HARDWARE\IIC\IOI2C.h

     文件       3761  2017-07-07 08:38  机器人机械臂最终版\MiniBalance_HARDWARE\KEY\key.c

     文件        605  2017-07-07 08:27  机器人机械臂最终版\MiniBalance_HARDWARE\KEY\key.h

     文件       1196  2016-06-05 14:52  机器人机械臂最终版\MiniBalance_HARDWARE\LED\LED.C

     文件        383  2016-06-05 14:52  机器人机械臂最终版\MiniBalance_HARDWARE\LED\LED.H

     文件       7636  2018-08-05 20:51  机器人机械臂最终版\MiniBalance_HARDWARE\MOTOR\motor.c

     文件        406  2018-08-05 19:31  机器人机械臂最终版\MiniBalance_HARDWARE\MOTOR\motor.h

     文件       6227  2016-06-05 14:52  机器人机械臂最终版\MiniBalance_HARDWARE\OLED\oled.c

     文件       1179  2016-06-05 14:52  机器人机械臂最终版\MiniBalance_HARDWARE\OLED\oled.h

     文件      16174  2013-09-13 11:45  机器人机械臂最终版\MiniBalance_HARDWARE\OLED\oledfont.h

     文件       4747  2017-10-20 15:25  机器人机械臂最终版\MiniBalance_HARDWARE\PS2\pstwo.c

     文件       1959  2017-10-20 15:25  机器人机械臂最终版\MiniBalance_HARDWARE\PS2\pstwo.h

     文件       6222  2017-02-21 21:32  机器人机械臂最终版\MiniBalance_HARDWARE\STMFLASH\stmflash.c

     文件       1550  2017-02-19 17:22  机器人机械臂最终版\MiniBalance_HARDWARE\STMFLASH\stmflash.h

     文件       1454  2018-08-05 21:18  机器人机械臂最终版\MiniBalance_HARDWARE\TIMER\timer.c

............此处省略101个文件信息