第一次写博客，难免会有些乱，大家见谅。
实验室最近在做一个足式机器人项目，我主要负责基于STM32F407ZGT6芯片控制板机器人的运动控制系统设计，打算用博客的形式记录自己的收获，机器人自下向上的总体架构是：机器人机械结构-》直流电机-》电机驱动器-》STM32F407控制板。项目处于前期程序模块设计阶段，首先需要验证电机的运动性能，用周期性的运动更容易获得结果，所以选择余弦信号，电机转动角度公式：
，由公式能够得出电机转动角度为A，求导后得到角速度公式：
，由公式可以看出电机的运行速度是从零开始增加的，可以减小电机启动负载。
本模块的最终目的是通过stm32控制板定时器的输出比较功能输出占空比变化的PWM波，该PWM波信号作为电机驱动器的输入控制信号，控制电机在正弦速度下运行（电机驱动器输入信号可以是PWM波，有速度闭环，并且我们项目还没有安装角度传感器，所以位置控制目前是开环的）。实现的思路是通过stm32的定时器设定电机运行步长时间steptime，然后累加运行时间runtime，带入上面速度公式，即可得到当前运动速度velocity_now，然后与电机最大运行速度做比，获得此时PWM波占空比，设置定时器的比较值ccr，这样就能让电机做三角函数跟随运动。
由于足式机器人的关节转动是作往复摆动运动，所以电机也需要验证正反转，我的解决办法是当速度小于零时，使能驱动器反转信号，使电机反转。程序的目的和思路都说完了，接下来就是具体的程序实现了。
首先是三角函数公式这部分，主要是得到当前时间电机的转速，该部分程序是可以通用的，由于我单腿是三个关机，所以有三个电机需要控制。
void motor_motion_sine（float steptime， float runtime， float Amplitude［3］， float Frequency［3］， float MotionTime， float MotorVel［3］）
{
float MotorSinW［3］;
MotorSinW［0］=2*pi*Frequency［0］;
MotorSinW［1］=2*pi*Frequency［1］;
MotorSinW［2］=2*pi*Frequency［2］;
float GaitTimeU=runtime;
if（GaitTimeU《MotionTime）
{
MotorVel［0］=MotorSinW［0］*Amplitude［0］*sin（MotorSinW［0］*GaitTimeU）;
MotorVel［1］=MotorSinW［1］*Amplitude［1］*sin（MotorSinW［1］*GaitTimeU）;
MotorVel［2］=MotorSinW［2］*Amplitude［2］*sin（MotorSinW［2］*GaitTimeU）;
}
}
运行时间我是用定时器来实现的，这样可以实现需要的控制周期，目前的控制周期是5ms，即200Hz，每5ms进入中断服务函数计算运行速度，并改变PWM波占空比，该中断服务函数后面会添加传感器数据采集处理、与上位机通信等模块。我用的是通用定时器TIM4，程序如下：
void TIM4_Int_Init（u16 arr，u16 psc）
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd（RCC_APB1Periph_TIM4，ENABLE）; ///使能TIM4时钟
TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值
TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit（TIM4，&TIM_TimeBaseInitStructure）;//初始化TIM4
TIM_ITConfig（TIM4，TIM_IT_Update，ENABLE）; //允许定时器4更新中断
TIM_Cmd（TIM4，ENABLE）; //使能定时器4
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init（&NVIC_InitStructure）;
}
//定时器4中断服务函数
static int countt=0;//variable for count
float motorvel［3］={0.0，0.0，0.0};//motor velocity
void TIM4_IRQHandler（void）
{
LED0=！LED0;
float ampl［3］={pi/4，pi/4，pi/4};// amplitude unit:V
float stptime=0.005;//steptime:5ms
float runt=0;
float freq［3］={0.5，0.5，0.5};//frequence
float motime=200;//motion time:10s
runt=countt*stptime;
countt++;
if（TIM_GetITStatus（TIM4，TIM_IT_Update）==SET） //溢出中断
{
LED1=！LED1;//DS1翻转
motor_motion_sine（stptime， runt， ampl， freq，motime， motorvel）;//调用速度公式函数
motor_open_excute（motorvel，500，freq，ampl）;//执行
}
TIM_ClearITPendingBit（TIM4，TIM_IT_Update）; //清除中断标志位
}
在中断服务函数中首先是定义了几个全局变量，供其他函数调用。然后定义了速度公式中所需的变量，作为速度公式函数的参数，motor_motion_sine函数用来计算当前速度，motor_open_excute函数是改变PWM波占空比改变电机速度，这个函数会在下一篇贴出。链接：STM32F407通过定时器输出多路PWM波控制电机做正反转的正弦运动（二）
文笔拙劣，大家多担待，如有问题，欢迎大家批评指正，我也是在自学，需要跟大家多交流学习，共同进步，感谢阅读。
第一次写博客，难免会有些乱，大家见谅。
实验室最近在做一个足式机器人项目，我主要负责基于STM32F407ZGT6芯片控制板机器人的运动控制系统设计，打算用博客的形式记录自己的收获，机器人自下向上的总体架构是：机器人机械结构-》直流电机-》电机驱动器-》STM32F407控制板。项目处于前期程序模块设计阶段，首先需要验证电机的运动性能，用周期性的运动更容易获得结果，所以选择余弦信号，电机转动角度公式：
，由公式能够得出电机转动角度为A，求导后得到角速度公式：
，由公式可以看出电机的运行速度是从零开始增加的，可以减小电机启动负载。
本模块的最终目的是通过stm32控制板定时器的输出比较功能输出占空比变化的PWM波，该PWM波信号作为电机驱动器的输入控制信号，控制电机在正弦速度下运行（电机驱动器输入信号可以是PWM波，有速度闭环，并且我们项目还没有安装角度传感器，所以位置控制目前是开环的）。实现的思路是通过stm32的定时器设定电机运行步长时间steptime，然后累加运行时间runtime，带入上面速度公式，即可得到当前运动速度velocity_now，然后与电机最大运行速度做比，获得此时PWM波占空比，设置定时器的比较值ccr，这样就能让电机做三角函数跟随运动。
由于足式机器人的关节转动是作往复摆动运动，所以电机也需要验证正反转，我的解决办法是当速度小于零时，使能驱动器反转信号，使电机反转。程序的目的和思路都说完了，接下来就是具体的程序实现了。
首先是三角函数公式这部分，主要是得到当前时间电机的转速，该部分程序是可以通用的，由于我单腿是三个关机，所以有三个电机需要控制。
void motor_motion_sine（float steptime， float runtime， float Amplitude［3］， float Frequency［3］， float MotionTime， float MotorVel［3］）
{
float MotorSinW［3］;
MotorSinW［0］=2*pi*Frequency［0］;
MotorSinW［1］=2*pi*Frequency［1］;
MotorSinW［2］=2*pi*Frequency［2］;
float GaitTimeU=runtime;
if（GaitTimeU《MotionTime）
{
MotorVel［0］=MotorSinW［0］*Amplitude［0］*sin（MotorSinW［0］*GaitTimeU）;
MotorVel［1］=MotorSinW［1］*Amplitude［1］*sin（MotorSinW［1］*GaitTimeU）;
MotorVel［2］=MotorSinW［2］*Amplitude［2］*sin（MotorSinW［2］*GaitTimeU）;
}
}
运行时间我是用定时器来实现的，这样可以实现需要的控制周期，目前的控制周期是5ms，即200Hz，每5ms进入中断服务函数计算运行速度，并改变PWM波占空比，该中断服务函数后面会添加传感器数据采集处理、与上位机通信等模块。我用的是通用定时器TIM4，程序如下：
void TIM4_Int_Init（u16 arr，u16 psc）
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd（RCC_APB1Periph_TIM4，ENABLE）; ///使能TIM4时钟
TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值
TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit（TIM4，&TIM_TimeBaseInitStructure）;//初始化TIM4
TIM_ITConfig（TIM4，TIM_IT_Update，ENABLE）; //允许定时器4更新中断
TIM_Cmd（TIM4，ENABLE）; //使能定时器4
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init（&NVIC_InitStructure）;
}
//定时器4中断服务函数
static int countt=0;//variable for count
float motorvel［3］={0.0，0.0，0.0};//motor velocity
void TIM4_IRQHandler（void）
{
LED0=！LED0;
float ampl［3］={pi/4，pi/4，pi/4};// amplitude unit:V
float stptime=0.005;//steptime:5ms
float runt=0;
float freq［3］={0.5，0.5，0.5};//frequence
float motime=200;//motion time:10s
runt=countt*stptime;
countt++;
if（TIM_GetITStatus（TIM4，TIM_IT_Update）==SET） //溢出中断
{
LED1=！LED1;//DS1翻转
motor_motion_sine（stptime， runt， ampl， freq，motime， motorvel）;//调用速度公式函数
motor_open_excute（motorvel，500，freq，ampl）;//执行
}
TIM_ClearITPendingBit（TIM4，TIM_IT_Update）; //清除中断标志位
}
在中断服务函数中首先是定义了几个全局变量，供其他函数调用。然后定义了速度公式中所需的变量，作为速度公式函数的参数，motor_motion_sine函数用来计算当前速度，motor_open_excute函数是改变PWM波占空比改变电机速度，这个函数会在下一篇贴出。链接：STM32F407通过定时器输出多路PWM波控制电机做正反转的正弦运动（二）
文笔拙劣，大家多担待，如有问题，欢迎大家批评指正，我也是在自学，需要跟大家多交流学习，共同进步，感谢阅读。
举报
