舵机平滑过渡处理

之前在控制机械臂的过程当中发现舵机转动过程中运动惯量比较大，且时间并不是完全可控，所以想通过算法来实现舵机的平滑运动处理，以及对时间的精准把控。

舵机平滑过渡处理函数:

 

 f(x)取值区间:自变量和阴变量都在[0,1]之间变化，其中自变量为时间，阴变量为对应输出的CCR值，需要改变其大小只需在函数后面乘上一个比例即可。

考虑到惯性原因，模拟出来的函数其求导f(x)'在自变量为0和1的时候都为0，即代表起始时刻和终止时刻其变化率都为0；确保其平滑过渡。

robotArmControl.cpp文件

#include<robotArmControl.h>
#include"stm32f1xx_hal.h"
namespace robotArm {
typedef struct {
	float angle;
	float last_angle;
	float d_angle;
	float change_time;
	float time;
	__IO uint32_t *CCRX;
} servoData;
float ratio[5];//ArshCoke
uint8_t servo_num = 0;
servoData servo[5];

/*
 * 初始化舵机控制器
 * 可填入每个舵机的最大角度(可不同)
 */
void robot_arm_initial_ArshCoke(uint8_t servoNum, float maxAngle[]){

	servo[0].angle = 1000;
	servo[0].last_angle = 1000;
	servo[0].CCRX = (__IO uint32_t *)0;

	servo[1].angle = 1000;
	servo[1].last_angle = 1000;
	servo[1].CCRX = (__IO uint32_t *)0;

	servo[2].angle = 1000;
	servo[2].last_angle = 1000;
	servo[2].CCRX = (__IO uint32_t *)0;

	servo[3].angle = 1000;
	servo[3].last_angle = 1000;
	servo[3].CCRX = (__IO uint32_t *)0;

	servo[4].angle = 1000;
	servo[4].last_angle = 1000;
	servo[4].CCRX = (__IO uint32_t *)0;

	servo_num = servoNum - 1;
	for(int i=0;i<servoNum;i++){
		ratio[i] = 2000.0 / maxAngle[i];//要求重装载值为20000,1us计数一次
	}

}

/*
 * 设置对应编号舵机的操作寄存器
 */
void setServoChannel_ArshCoke(uint8_t num,__IO uint32_t *CCRX) {
	servo[num].CCRX = CCRX;
}


/*
 * 设置target号舵机以time ms 从当期位置转到angle
 */
void setTargetAngle_ArshCoke(float angle, float time, uint8_t target){
	angle = angle * ratio[target] + 500; //转换为比较值(目标CCRX的值)
	servo[target].last_angle = servo[target].angle;
	servo[target].angle = angle;
	servo[target].change_time = 1.0f / time; //计算时间增量
	servo[target].time = 0;
	servo[target].d_angle = angle - servo[target].last_angle;
}


/*
 * 定时器定时执行函数，用于动作实现
 * 要求此函数每隔1ms执行
 */
void actuator_ArshCoke() {
	int i;
	for(i = 0;i <= servo_num;i++){
		if(servo[i].CCRX == 0){
			break;
		}
		if (servo[i].time <= 1) {
			(*servo[i].CCRX) = (uint16_t) (((
					6 * servo[i].time * servo[i].time* servo[i].time * servo[i].time * servo[i].time
					- 15 * servo[i].time * servo[i].time * servo[i].time * servo[i].time
					+ 10 * servo[i].time * servo[i].time * servo[i].time)* servo[i].d_angle) + servo[i].last_angle);
			servo[i].time += servo[i].change_time;
		}
	}
}
}

robotArmControl.h文件:

#include"stm32f1xx_hal.h"
#ifndef ROBOTARMCONTROL_H_
#define ROBOTARMCONTROL_H_

namespace robotArm{
	void robot_arm_initial_ArshCoke(uint8_t servoNum,float maxAngle);
	void setServoChannel_ArshCoke(uint8_t num,__IO uint32_t* CCRX);
	void setTargetAngle_ArshCoke(float angle,float time,uint8_t target);
	void actuator_ArshCoke();
}



#endif /* ROBOTARMCONTROL_H_ */

注:本文仅是本人学习记录使用故没有展开说明，若有不懂的地方需要解答请留言或者联系QQ:1362501977

未经本人允许，禁止转载。