对步科增量式举升电机进行加减速梯形控制

背景：自己移植代码后，可以实现举升控制，但速度是恒定的，查找bug的思路
解决：1、自己单位换算有问题；2、手动和举升任务出现干扰
应用工具：1、CAN分析仪过滤数据；2、步科上位机机监控速度变化；
1）机械参数参数：
举升电机SMC80S-0075-30ABK-3DKH
减速比10
导程10
最大有效形成686（实际测量600）
2）公式
电机转速转丝杠行程速度 rpm ——> mm/s

rotate * motor_param.lead_screw / (motor_param.reduction_ratio * motor_param.teeth_ratio * 60.0f)

丝杠行程速度转电机转速 mm/s ——> rpm

speed * (motor_param.reduction_ratio * motor_param.teeth_ratio * 60.0f) / motor_param.lead_screw

下面正向逆向运算（公式中的齿数比为1）

二、具体程序中实现
2.1需要电机实时高度数据，步科增量式电机：编码器精度是10000，那速度就是2730=1rpm
此时的实时高度由拉线编码器获取，拉线获取值为mm;

梯形速度控制


具体速度输出


三、遇到问题时如何排查
3.1查询速度单位不统一
打断点实时监控自己下发的速度，与实际电机（步科上位机获取的速度）做对比->自己下发速度特别小0.05->查询上方计算加减速部分；
发现是在自己限制幅度内，说明自己计算的速度值有问题，仔细查看单位弄错了；
3.2速度正常下发后，立即减速，应该是缓慢减速的，而且还有异响
初步判断：有0和减速度来回切换导致的；数据依据：通过CAN卡抓取数据，分析得出减速时203帧ID筛选出来的，一帧0一帧其他速度；
自己速度下发，由最根本往上查找，发现手动也调用该速度下发函数了，问题得以解决；
四、具体代码

void LoadingUnload_Task(void const * argument)
{
	uint32_t PreviousWakeTime;
	uint64_t current_time_ms = 0;

//	int32_t current_pose=0;
//	float current_speed=0;
	platform_command.platform_Lift_CMD = STOP;


	PreviousWakeTime = osKernelSysTick();
	while(1)
	{
		osDelayUntil(&PreviousWakeTime,10);
		xEventGroupSetBits(EventGroupHandler_WWDG,EVENTBIT_AXIS_TASK);

		if(OperatingStatus == EMY_STOP)
		{

		}
		else
		{
			/*计算平台当前高度和当前角度*/
			Axis_Motor_Feedback();
			if( fabs(encoder1_length-platform_par.platform_max_height) < 0.004f) //拉线编码器读取数值在举升丝杆最大行程附近
			{
				platform_status.platform_height_flag = TOP;
			}
			else if(fabs(encoder1_length) < 0.004f)  //平台位于最低位 && Platform_Down_Limit
			{
				platform_status.platform_height_flag = LOWEST;
			}
			else  //平台位于中间位置
			{

			}
			/***************************各料轴货物计数*****************************************/



			Photoelectric_State_Updata();  //获取料轴光电信号；
			/***************************取放货处理*****************************************/
			if(auto_mannual_switch == (GPIO_PinState)MANUAL_MODE) //手动模式
			{
				if(Mannual_load_up == 0) //升叉
				{
					TrapezoidalVelocity_Control(PlatformUpDownMotor,platform_par.platform_max_height,&platform_command.platform_LiftSpeed_set,0.01f);
				}
				else if(Mannual_load_down == 0)//降叉
				{
					TrapezoidalVelocity_Control(PlatformUpDownMotor,0.0f,&platform_command.platform_LiftSpeed_set,0.01f);
				}
				else
				{
					ActionStop_f(&platform_command.platform_LiftSpeed_set,0.5f);
				}
			}
			else
			{
//				Agv_MotorSpeed_Set(PlatformUpDownMotor,0.0f);
			}
			Agv_MotorSpeed_Set(PlatformUpDownMotor,-platform_command.platform_LiftSpeed_set * 60.0f / platform_par.platform_lift_screwLead * platform_par.platform_lift_reduction_ratio);
		}
			/***************************传感器状态上报*****************************************/
		if(current_time_ms % 10 == 0)
			Sensors_statues_response();
		current_time_ms ++;
	}
}

//上装平台相关参数定义
const Platform_ParaStruct platform_par = {
	.platform_lift_encoderZero = 0,
	.platform_spin_encoderZero = 0,
	.platform_max_height = 600.0f,                //mm丝杆行程 0.63
	.platform_max_encoder = 2730.0f,//8222500/526,//65536/360*270,//8222500
	.platform_max_speed = 10000.0f,
	.platform_rotate_maxencoding = 59964.2f,  //65536.0f*64.0f*188.0f/25.0f/526.0f,
	.platform_rotate_gear_ratio = 188.0f / 25.0f,
	.platform_rotate_reduction_ratio = 64,
	.platform_lift_reduction_ratio = 10.0f,       //减速比
	.platform_lift_screwLead = 10.0f              //丝杠导程
};
Platform_CommandStruct platform_command;
Platform_StatusStruct  platform_status;

/*******************************************************************************
* 函数名 : uint8_t TrapezoidalVelocity_Control(uint8_t MotorNum,float targetValue,float *speed,float period)
* 描  述 : 梯形速度控制
* 输  入 : MotorNum：需要控制的轴
*		targetValue：目标值
*		*speed：速度
*		period：周期
* 输  出 : *speed：计算后的速度
* 返回值 : 无
*******************************************************************************/
float distance_Remaining1=0;
uint8_t TrapezoidalVelocity_Control(uint8_t MotorNum,float targetValue,float *speed,float period)
{
	//加速度调整值   刹车距离   刹车距离+1个周期距离   加速距离
	float acc_Adjust,distance_Braking,distance_v_Brake,distance_acc_Brake;
	float dec_DrivMotor,acc_DrivMotor,reci_acc_DrivMotor,distance_Remaining,deadBand,V_MAX;
	uint8_t movingDireciton;
	switch(MotorNum)
	{
		//平台升降电机速度控制
		case PlatformUpDownMotor:
			targetValue =MaxMinLimit(targetValue,0.0,platform_par.platform_max_height);
//			distance_Remaining = (targetValue - platform_status.platform_current_height) * 1000.0f;     //绝对值电机获取高度 单位:mm
			distance_Remaining = targetValue - lift_CurrentDistance;                        //拉线编码器获取高度 单位:mm
			distance_Remaining1 = distance_Remaining;
			V_MAX = (2500.0f * platform_par.platform_lift_screwLead / (platform_par.platform_lift_reduction_ratio * 60.0f));
			acc_DrivMotor = (V_MAX * period);//mm/ti
			dec_DrivMotor = acc_DrivMotor*1.5;//mm/Ti
			reci_acc_DrivMotor = 0.01f / dec_DrivMotor;
			deadBand = 1.0f;
			break;

		//平台自旋电机速度控制
		case PlatformRotateMotor:
			if(targetValue > Pi || targetValue < -Pi)
			{
				*speed = 0;
				return 0;
			}
			distance_Remaining = -(targetValue - platform_status.platform_current_angle);	//单位:rad 旋转平台
			AngleRangePI_PI(distance_Remaining);
			acc_DrivMotor = 0.003f;
			dec_DrivMotor = 0.001f;
			V_MAX = 0.6f;  //对应电机最大2800rpm
			reci_acc_DrivMotor = 0.01f / acc_DrivMotor;
			deadBand = 0.02f;
			break;

		default:
			return 0;
	}
	if(distance_Remaining > 0)
		movingDireciton = 0;
	else
		movingDireciton = 1;

	distance_Remaining = fabs(distance_Remaining);
	if(distance_Remaining <= deadBand)
	{
		if(movingDireciton == 0)
		{
			*speed -= dec_DrivMotor*2;
			if(*speed <= 0)
			{
				*speed = 0;
				return true;
			}
		}
		else
		{
			*speed += dec_DrivMotor*2;
			if(*speed >= 0)
			{
				*speed = 0;
				return true;
			}
		}
	}
	else
	{
		distance_Braking = 0.5f * (*speed) * (*speed) * reci_acc_DrivMotor;
		distance_v_Brake = distance_Braking + fabs(*speed) * period;
		distance_acc_Brake = distance_v_Brake + 0.5f * acc_DrivMotor * period;

		if(distance_Remaining >= distance_acc_Brake)
		{
			if(movingDireciton == 0)
			{
				*speed += acc_DrivMotor;
				if(*speed > V_MAX)
					*speed = V_MAX;
			}
			else
			{
				*speed -= acc_DrivMotor;
				if(*speed < -V_MAX)
					*speed = -V_MAX;
			}
		}
		else if(distance_Remaining < distance_v_Brake)
		{
			acc_Adjust = acc_DrivMotor * distance_Braking / distance_Remaining;
			if(movingDireciton == 0)
			{
				*speed -= acc_Adjust;
				if(*speed < 0)
					*speed = 0;
			}
			else
			{
				*speed += acc_Adjust;
				if(*speed > 0)
					*speed = 0;
			}
		}
	}
	return 0;
}

/*******************************************************************************
* 函数名 : void ActionStop(float *PresentSpeed,int16_t Dec)
* 描  述 : 停止动作
* 输  入 : *PresentSpeed：当前速度
*					 Dec：减速度
* 输  出 : *PresentSpeed：减速后的速度
* 返回值 : 无
*******************************************************************************/
void ActionStop_f(float *PresentSpeed,float Dec)
{
	if(fabs(*PresentSpeed) <= Dec)
	{
		*PresentSpeed = 0.0f ;
	}
	else if(*PresentSpeed > 0)
	{
		*PresentSpeed -= Dec;
	}
	else if(*PresentSpeed < 0)
	{
		*PresentSpeed += Dec;
	}
}

/*******************************************************************************
* 函数名  :void Axis_Motor_Feedback(void)
* 描述     :获取轴位置及速度反馈信息
* 输入     : 无
* 输出     : 无
* 返回值  : 无
* 说明      : 无
*******************************************************************************/
void Axis_Motor_Feedback(void){
	int32_t current_pose = 0;
	float cur_speed = 0.0f;

	lift_CurrentDistance = encoder1_length ;  //拉线编码器实时获取的高度mm

	Agv_MotorPose_Get(PlatformUpDownMotor,&current_pose);
	lift_motor_curdis = Calculate_ScrewCurrentDistance(current_pose,robot_param.lift_motor);
	lift_motor_curdis *= 15.426f;	          //实际高度对应增量式电机的换算系数（实际测量获取系数）
	Agv_MotorSpeed_Get(PlatformUpDownMotor,&cur_speed);
//	wholelift_speed_feed = MotorSpeed2ScrewSpeed(cur_speed,robot_param.lift_motor);
}

具体速度下发

/**
 *@brief	电机速度设置
 *@param	无
 *@return	无
 */
void Agv_MotorSpeed_Set(Motor_Index motor_index,float speed){
	switch(motor_index){
	case WalkLeftMotor:
		Motor_Speed_Set(_motor_type,SERVO_1_RPDO1, speed);
		break;
	case WalkRightMotor:
		Motor_Speed_Set(_motor_type,SERVO_2_RPDO1, speed);
		break;
	case PlatformUpDownMotor:
		Motor_Speed_Set(_motor_type,SERVO_3_RPDO1, speed);
		break;
	case PlatformRotateMotor:
//		Motor_Speed_Set(_motor_type,SERVO_4_RPDO1, speed / RPM_INNER_CONVERT_KincoINC * 17895.67f);
		break;
	default:
		Sys_PeripheralErrorSet(EP_MotorType,true);
	}
}

/**
 *@brief	电机速度下发
 *@param	无
 *@return	无
 */
void Motor_Speed_Set(uint8_t _motor_type,uint32_t pdo_id,float speed)
{
	uint8_t _send_data[8];
	int32_t speed_value;
	if(speed > 3000)
		speed = 3000;
	else if(speed < -3000)
		speed = -3000;

	switch(_motor_type){
	case Kinco:
		speed_value = speed * RPM_INNER_CONVERT_KincoINC;  //增量式电机
		break;

	case Tongyi:
		if(speed > 2500)
			speed = 2500;
		else if(speed < -2500)
			speed = -2500;
		speed_value = (int32_t)speed * 10.0f;
		break;
	default:
		Sys_PeripheralErrorSet(EP_MotorType,true);
	}
	memcpy(_send_data,&speed_value,8);
	MOTOR_SEND_MSG(pdo_id,8,CAN_RTR_DATA,_send_data);
}

CAN底层发送数据

/* USER CODE BEGIN 1 */
uint8_t can1_send_msg(uint32_t StdId, uint32_t DLC, uint32_t RTR, uint8_t *Data)
{
	static uint8_t led_cnt=0;
	CAN_TxPacketTypeDef packet;
	CAN_TxHeaderTypeDef  CAN_RxHeaderVal;
	uint64_t timout_cnt=0;
	CAN_RxHeaderVal.StdId = StdId;
	CAN_RxHeaderVal.IDE = 0;
	CAN_RxHeaderVal.RTR = RTR;
	CAN_RxHeaderVal.DLC = DLC;
	CAN_RxHeaderVal.TransmitGlobalTime = DISABLE;
	for(uint8_t i=0;i<DLC;i++)
		packet.payload[i] = Data[i];
	while( HAL_CAN_GetTxMailboxesFreeLevel( &hcan1 ) == 0 )
	{
		timout_cnt++;
		if(timout_cnt>MAX_DELAY)return 0;
	}
	if(led_cnt++>100){
		CAN1_TX_LIGHT_TOGGLE;
		led_cnt=0;
	}
//	vTaskDelay(3);
	if(HAL_CAN_AddTxMessage(&hcan1, &CAN_RxHeaderVal, packet.payload, &packet.mailbox) != HAL_OK){
		return 1;
	}
	return 0;
}