步进电机控制机械臂源码 代码

步进电机控制机械臂源码 代码 步进电机控制机械臂源码 代码步进电机控制机械臂源码 代码

#include “control.h“	
  /**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
int Balance_PwmVelocity_PwmTurn_Pwm;
u8 Flag_TargetStep=0;
u32 Flash_R_Count;
int Voltage_TempVoltage_CountVoltage_All;
float Last_Position_ALast_Position_BLast_Position_C;
/***************************************************
**************************************************************************/
#define a            （0.14f）                   
#define b            （0.16f） 
#define h            （0.097f） 
#define L            （0.06f） 
/**************************************************************************
函数功能：运动数学模型
入口参数：X Y Z 三轴速度或者位置
返回  值：无
**************************************************************************/
void Kinematic_Analysis（int Pxint Pyint Pz）
{ 
	      float target_xtarget_ytarget_z;
	      float cde;
	      target_x=（float）Px/1000;
				target_y=（float）Py/1000;
				target_z=（float）Pz/180*PI;
	      c=target_x-L;
				d=target_y-h;
	      e=sqrt（pow（d2）+pow（c2））;  
		    Target_A   =acos（（a*a+e*e-b*b）/2/a/e）+atan（d/c）;
        Target_B   =Target_A-PI+acos（（a*a+b*b-e*e）/2/a/b）;
				Target_C   =-target_z;

				Target_A*=-RATIO;
				Target_B*=-RATIO;
				Target_C*=-RATIO;
}

/**************************************************************************
函数功能：所有的控制代码都在这里面
         5ms定时中断由MPU6050的INT引脚触发
         严格保证采样和数据处理的时间同步				 
**************************************************************************/
int EXTI15_10_IRQHandler（void） 
{    
	 if（INT==0）		
	{   
		   EXTI->PR=1<<15;                                                      //清除中断标志位   
		   Flag_Target=!Flag_Target;
			 	Key（）;                                        //===扫描按键状态 单击双击可以改变小车运行状态
				Position_A=-Read_Encoder（3）;            	 		//===读取编码器的值
				Position_B=Read_Encoder（2）;           		 		//===读取编码器的值
				Position_C=Read_Encoder（4）;            	 	  	//===读取编码器的值
				Encoder_A=Position_A-Last_Position_A;         //===一介差分得到速度值
				Encoder_B=Position_B-Last_Position_B;         //===一介差分得到速度值
				Encoder_C=Position_C-Last_Position_C;         //===一介差分得到速度值
			 	if（Usart_Flag==0&&Usart_ON_Flag==1）  memcpy（rxbufUrxbuf8*sizeof（u8））;	//如果解锁了串口控制标志位，进入串口控制模式
				Get_Angle（Way_Angle）;                                                   //===更新姿态	
				if（CAN_ON_Flag==1||Usart_ON_Flag==1） CAN_N_Usart_Control（）;          //接到串口或者CAN遥控解锁指令之后，使能CAN和串口控制输入
				if（Xianfu_XYZ（））          //不超过运动范围且不在拖动示教模式下，执行运动学分析
				Kinematic_Analysis（Target_XTarget_YTarget_Z）; //根据给定坐标，解算出每个电机的位置

				Motor_A=Position_PID_A（Position_ATarget_A）>>5;//A电机位置控制
				Motor_B=Position_PID_B（Position_BTarget_B）>>5;//B电机位置控制
				Motor_C=-Position_PID_C（Position_CTarget_C）>>5;//C电机位置
        Count_Velocity（）;

				if（（Position_A-Position_B>-3120）&&myabs（Encoder_A）>myabs（Encoder_B））Moto

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-02-10 16:06  1.步进电机版机械臂源码（标准版）\
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\CONTROL\
     文件       13270  2017-08-28 20:01  1.步进电机版机械臂源码（标准版）\MiniBalance\CONTROL\control.c
     文件        1213  2017-08-22 19:13  1.步进电机版机械臂源码（标准版）\MiniBalance\CONTROL\control.h
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\
     文件       19342  2012-12-14 11:16  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\dmpKey.h
     文件        6765  2012-12-14 11:16  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\dmpmap.h
     文件       84104  2015-07-09 14:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\inv_mpu.c
     文件        4335  2015-07-09 14:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\inv_mpu.h
     文件       57839  2015-07-09 14:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\inv_mpu_dmp_motion_driver.c
     文件        3386  2015-07-09 14:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DMP\inv_mpu_dmp_motion_driver.h
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DataScope_DP\
     文件        2891  2015-07-09 14:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DataScope_DP\DataScope_DP.C
     文件         586  2015-07-09 14:05  1.步进电机版机械臂源码（标准版）\MiniBalance\DataScope_DP\DataScope_DP.h
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\MPU6050\
     文件       11303  2017-08-28 14:57  1.步进电机版机械臂源码（标准版）\MiniBalance\MPU6050\MPU6050.c
     文件       14985  2017-08-28 14:57  1.步进电机版机械臂源码（标准版）\MiniBalance\MPU6050\mpu6050.h
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\filter\
     文件        2105  2015-12-07 15:52  1.步进电机版机械臂源码（标准版）\MiniBalance\filter\filter.c
     文件         399  2016-04-06 14:10  1.步进电机版机械臂源码（标准版）\MiniBalance\filter\filter.h
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance\show\
     文件        3982  2017-08-28 14:57  1.步进电机版机械臂源码（标准版）\MiniBalance\show\show.c
     文件         354  2017-04-20 18:37  1.步进电机版机械臂源码（标准版）\MiniBalance\show\show.h
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance_COER\
     文件       17273  2010-06-07 10:25  1.步进电机版机械臂源码（标准版）\MiniBalance_COER\core_cm3.c
     文件       85714  2011-02-09 14:59  1.步进电机版机械臂源码（标准版）\MiniBalance_COER\core_cm3.h
     文件       15503  2011-03-10 10:52  1.步进电机版机械臂源码（标准版）\MiniBalance_COER\startup_stm32f10x_hd.s
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance_HARDWARE\
     目录           0  2018-02-10 16:05  1.步进电机版机械臂源码（标准版）\MiniBalance_HARDWARE\ADC\
     文件        2932  2017-08-21 12:20  1.步进电机版机械臂源码（标准版）\MiniBalance_HARDWARE\ADC\adc.c
............此处省略293个文件信息