|
背景：自己移植代码后，可以实现举升控制，但速度是恒定的，查找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 = 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) 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,¤t_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;iMAX_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;
}
|