S曲线C语言实现，利用robomodule+STM32F429+直流伺服电机进行简单验证
·
之前我一直用的板子是stm32f407的，电机驱动程序也是基于该芯片的，所以换成429之后首先需要进行程序移植，主要是CAN通讯的程序有些许区别。
（1）f407的主频为168MHz，而f429的主频为180MHz，要修改 CAN 的 CAN_BSx 寄存器，使主控程序上的波特率为1M，可按如下表格进行修改设置：
（2）GPIO时钟树发生改变，引脚复用映射配置发生改变。
总的CAN通讯初始化配置如下：
void CAN1_Configuration(void)
{
CAN_InitTypeDef can;
CAN_FilterInitTypeDef can_filter;
GPIO_InitTypeDef gpio;
NVIC_InitTypeDef nvic;
//使能相关时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);//使能gpio时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);//使能CAN1时钟
//引脚复用映射配置 Connect CAN pins to AF9
GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_CAN1);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_CAN1);
//初始化GPIO
gpio.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_8;
gpio.GPIO_Mode = GPIO_Mode_AF;//复用功能
GPIO_Init(GPIOB, &gpio);//初始化PB8, PB9
nvic.NVIC_IRQChannel = CAN1_RX0_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = 2;//主优先级为2
nvic.NVIC_IRQChannelSubPriority = 1;// 次优先级为1
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
nvic.NVIC_IRQChannel = CAN1_TX_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = 1;
nvic.NVIC_IRQChannelSubPriority = 1;
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
//CAN寄存器初始化
CAN_DeInit(CAN1);
CAN_StructInit(&can);
//CAN单元设置
can.CAN_TTCM = DISABLE;//非时间触发通信模式
can.CAN_ABOM = DISABLE;//软件自动离线管理
can.CAN_AWUM = DISABLE;//睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位)
can.CAN_NART = DISABLE;//报文自动传送 %%%%%%%%%%
can.CAN_RFLM = DISABLE;//报文不锁定,新的覆盖旧的
can.CAN_TXFP = ENABLE;//%%%%%%%%%%%
can.CAN_Mode = CAN_Mode_Normal;//常规模式
can.CAN_SJW = CAN_SJW_1tq;//再同步跳转宽度
can.CAN_BS1 = CAN_BS1_5tq;//时间段1占用5个时间单元数
can.CAN_BS2 = CAN_BS2_3tq;//时间段2占用3个时间单元数
can.CAN_Prescaler = 5; //CAN BaudRate 45/(1+5+3)/5=1Mbps 分频系数
CAN_Init(CAN1, &can);// 初始化CAN1
//配置过滤器，可配置过滤器来接收固定的几个id
can_filter.CAN_FilterNumber = 0; //过滤器0
can_filter.CAN_FilterMode = CAN_FilterMode_IdMask;
can_filter.CAN_FilterScale = CAN_FilterScale_32bit;//32位
can_filter.CAN_FilterIdHigh = 0x0000;////32位ID
can_filter.CAN_FilterIdLow = 0x0000;
can_filter.CAN_FilterMaskIdHigh = 0x0000;//32位MASK
can_filter.CAN_FilterMaskIdLow = 0x0000;
can_filter.CAN_FilterFIFOAssignment = 0;//过滤器0关联到FIFO0
can_filter.CAN_FilterActivation=ENABLE;//激活过滤器0
CAN_FilterInit(&can_filter);//滤波器初始化
CAN_ITConfig(CAN1,CAN_IT_FMP0,ENABLE);
CAN_ITConfig(CAN1,CAN_IT_TME,ENABLE);
}
接下来是S曲线的C语言代码，准备知识及相应的matlab代码见机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划。
/* scurve.h */
#ifndef _SCURVE_H_
#define _SCURVE_H_
typedef struct SCurve_Node *PtrToSCurve;
struct SCurve_Node
{
float Ta;
float Tv;
float Td;
float Tj1;
float Tj2;
float q0;
float q1;
float v0;
float v1;
float vlim;
float amax;
float amin;
float alima;
float alimd;
float jmax;
float jmin;
};
typedef PtrToSCurve SCurve_para;
int sign(float q0, float q1);
// S曲线参数计算
void STrajectoryPara(SCurve_para Input_data, float q0, float q1, float v0, float v1, float vmax, float amax, float jmax);
// 计算位置
float S_position(float t, SCurve_para Input_data);
// 计算速度
float S_velocity(float t, SCurve_para Input_data);
// 计算加速度
float S_acceleration(float t, SCurve_para Input_data);
// 计算加加速度
float S_jerk(float t, SCurve_para Input_data);
#endif
/* scurve.c */
#include "Headfile.h"
int sign(float q0, float q1)
{
int a = 0;
if (q1 - q0 > 0)
a = 1;
else
a = -1;
return a;
}
// S曲线参数计算
void STrajectoryPara(SCurve_para Input_data, float q0, float q1, float v0, float v1, float vmax, float amax, float jmax)
{
float vmin, amin, jmin;
int sigma = 0;
float Tj = 0, delta = 0;
vmin = -vmax;
amin = -amax;
jmin = -jmax;
// 利用公式（3.31）（3.32）转化得到实际的q_0、q_1、v_max、a_max
sigma = sign(q0, q1);
Input_data->q0 = sigma * q0;
Input_data->q1 = sigma * q1;
Input_data->v0 = sigma * v0;
vmax = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
vmin = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
Input_data->amax = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
Input_data->amin = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
Input_data->jmax = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
Input_data->jmin = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;
// 判断是否达到最大速度
if ((vmax - Input_data->v0)*Input_data->jmax < pow(Input_data->amax, 2)){
// 达不到amax
Input_data->Tj1 = sqrt((vmax - Input_data->v0)/Input_data->jmax);
Input_data->Ta = 2*Input_data->Tj1;
Input_data->alima = Input_data->jmax * Input_data->Tj1;
}else{
// 能够达到amax
Input_data->Tj1 = Input_data->amax/Input_data->jmax;
Input_data->Ta = Input_data->Tj1 + (vmax - Input_data->v0 )/Input_data->amax;
Input_data->alima = Input_data->amax;
}
if ((vmax - Input_data->v1)*Input_data->jmax < pow(Input_data->amax, 2)){
// 达不到amin
Input_data->Tj2 = sqrt((vmax - Input_data->v1)/Input_data->jmax);
Input_data->Td = 2 * Input_data->Tj2;
Input_data->alimd = -Input_data->jmax * Input_data->Tj2;
}else{
// 能够达到amin
Input_data->Tj2 = Input_data->amax/Input_data->jmax;
Input_data->Td = Input_data->Tj2 + (vmax - Input_data->v1)/Input_data->amax;
Input_data->alimd = -Input_data->amax;
}
// 根据（3.25）计算匀速段时间
Input_data->Tv = (Input_data->q1 - Input_data->q0)/vmax - (Input_data->Ta/2)*(1 + Input_data->v0/vmax)
- (Input_data->Td/2)*(1 + Input_data->v1/vmax);
// 对Tv进行讨论
if (Input_data->Tv > 0)
// 能够达到给定的最大速度vmax， 即存在匀速阶段
Input_data->vlim = vmax;
else{
// 达不到最大速度，即匀速阶段Tv=0
// 假设最大加速度和最小加速度均能达到
Input_data->Tv = 0;
Tj = Input_data->amax / Input_data->jmax;
Input_data->Tj1 = Tj;
Input_data->Tj2 = Tj;
delta = (pow(Input_data->amax, 4)/pow(Input_data->jmax, 2)) + 2*(pow(Input_data->v0, 2) + pow(Input_data->v1, 2))
+ Input_data->amax*(4*(Input_data->q1 - Input_data->q0) - 2*(Input_data->amax/Input_data->jmax)*(Input_data->v0 + Input_data->v1));
Input_data->Ta = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v0 + sqrt(delta)) / (2*Input_data->amax);
Input_data->Td = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v1 + sqrt(delta)) / (2*Input_data->amax);
// 对Ta和Td进行讨论
if (Input_data->Ta < 0 || Input_data->Td < 0){
if (Input_data->Ta < 0){
// 没有加速段，只有减速段
Input_data->Ta = 0;
Input_data->Tj1 = 0;
Input_data->Td = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj2 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
+ pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = 0;
Input_data->alimd = -Input_data->jmax * Input_data->Tj2;
Input_data->vlim = v0;
}else if(Input_data->Td < 0){
// 没有减速段，只有加速段
Input_data->Td = 0;
Input_data->Tj2 = 0;
Input_data->Ta = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj1 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
- pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = Input_data->jmax * Input_data->Tj1;
Input_data->alimd = 0;
Input_data->vlim = Input_data->jmax * Input_data->Tj1;
}
}else if(Input_data->Ta >= 2*Tj && Input_data->Td >= 2*Tj){
// 加速段和减速段都能达到最大速度
Input_data->alima = Input_data->amax;
Input_data->alimd = -Input_data->amax;
Input_data->vlim = v0 + Input_data->alima*(Input_data->Ta - Tj);
}else{
// 加速段和减速阶段至少有一段不能达到最大加速度
float lambda = 0.99; // 系统取0<lambda<1
while (Input_data->Ta < 2*Tj || Input_data->Td < 2*Tj){
Input_data->amax = lambda * Input_data->amax;
Input_data->Tv = 0;
Tj = Input_data->amax / Input_data->jmax;
Input_data->Tj1 = Tj;
Input_data->Tj2 = Tj;
delta = (pow(Input_data->amax, 4)/pow(Input_data->jmax, 2)) + 2*(pow(Input_data->v0, 2) + pow(Input_data->v1, 2))
+ Input_data->amax*(4*(Input_data->q1 - Input_data->q0) - 2*(Input_data->amax/Input_data->jmax)*(Input_data->v0 + Input_data->v1));
Input_data->Ta = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v0 + sqrt(delta)) / (2*Input_data->amax);
Input_data->Td = ((pow(Input_data->amax, 2)/Input_data->jmax) - 2*Input_data->v1 + sqrt(delta)) / (2*Input_data->amax);
if (Input_data->Ta < 0 || Input_data->Td < 0){
if (Input_data->Ta < 0){
// 没有加速段，只有减速段
Input_data->Ta = 0;
Input_data->Tj1 = 0;
Input_data->Td = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj2 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
+ pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = 0;
Input_data->alimd = -Input_data->jmax * Input_data->Tj2;
Input_data->vlim = v0;
}else if(Input_data->Td < 0){
// 没有减速段，只有加速段
Input_data->Td = 0;
Input_data->Tj2 = 0;
Input_data->Ta = 2*(Input_data->q1 - Input_data->q0) / (Input_data->v0 + Input_data->v1);
Input_data->Tj1 = (Input_data->jmax*(Input_data->q1 - Input_data->q0) - sqrt(Input_data->jmax*(Input_data->jmax*pow(Input_data->q1 - Input_data->q0, 2)
- pow(Input_data->v1 + Input_data->v0, 2)*(Input_data->v1 - Input_data->v0)))) / (Input_data->jmax*(Input_data->v1 + Input_data->v0));
Input_data->alima = Input_data->jmax * Input_data->Tj1;
Input_data->alimd = 0;
Input_data->vlim = Input_data->jmax * Input_data->Tj1;
}
}else if(Input_data->Ta >= 2*Tj && Input_data->Td >= 2*Tj){
// 加速段和减速段都能达到最大速度
Input_data->alima = Input_data->amax;
Input_data->alimd = -Input_data->amax;
Input_data->vlim = v0 + Input_data->alima*(Input_data->Ta - Tj);
}
}
}
}
}
// 计算位置
float S_position(float t, SCurve_para Input_data)
{
float T = 0, q = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float q0 = Input_data->q0;
float q1 = Input_data->q1;
float v0 = Input_data->v0;
float v1 = Input_data->v1;
float vlim = Input_data->vlim;
float alima = Input_data->alima;
float alimd = Input_data->alimd;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
q = q0 + v0*t + jmax*pow(t, 3)/6;
else if (t >= Tj1 && t < Ta - Tj1)
q = q0 + v0*t +(alima/6)*(3*pow(t, 2) - 3*Tj1*t + pow(Tj1, 2));
else if (t >= Ta - Tj1 && t < Ta)
q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*(pow(Ta - t, 3)/6);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(pow(t - T + Td, 3)/6);
else if (t >= T - Td + Tj2 && t < T - Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*pow(t - T + Td, 2) - 3*Tj2*(t - T + Td) + pow(Tj2, 2));
else if (t >= T - Tj2 && t <= T)
q = q1 - v1*(T - t) - jmax*(pow(T - t, 3)/6);
return q;
}
// 计算速度
float S_velocity(float t, SCurve_para Input_data)
{
float T = 0, qd = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float v0 = Input_data->v0;
float v1 = Input_data->v1;
float vlim = Input_data->vlim;
float alima = Input_data->alima;
float alimd = Input_data->alimd;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qd = v0 + jmax*(pow(t, 2)/2);
else if (t >= Tj1 && t < Ta - Tj1)
qd = v0 + alima*(t - Tj1/2);
else if (t >= Ta - Tj1 && t < Ta)
qd = vlim + jmin*(pow(Ta - t, 2)/2);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qd = vlim;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qd = vlim - jmax*(pow(t - T + Td, 2)/2);
else if (t >= T - Td + Tj2 && t < T - Tj2)
qd = vlim + alimd*(t - T + Td - Tj2/2);
else if (t >= T - Tj2 && t <= T)
qd = v1 + jmax*(pow(t - T, 2)/2);
return qd;
}
// 计算加速度
float S_acceleration(float t, SCurve_para Input_data)
{
float T = 0, qdd = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float alima = Input_data->alima;
float alimd = Input_data->alimd;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qdd = jmax * t;
else if (t >= Tj1 && t < Ta - Tj1)
qdd = alima;
else if (t >= Ta - Tj1 && t < Ta)
qdd = -jmin * (Ta - t);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qdd = 0;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qdd = -jmax * (t - T - Td);
else if (t >= T - Td + Tj2 && t < T - Tj2)
qdd = alimd;
else if (t >= T - Tj2 && t <= T)
qdd = -jmax * (T - t);
return qdd;
}
// 计算加加速度
float S_jerk(float t, SCurve_para Input_data)
{
float T = 0, qddd = 0;
float Ta = Input_data->Ta;
float Tv = Input_data->Tv;
float Td = Input_data->Td;
float Tj1 = Input_data->Tj1;
float Tj2 = Input_data->Tj2;
float jmax = Input_data->jmax;
float jmin = Input_data->jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qddd = jmax;
else if (t >= Tj1 && t < Ta - Tj1)
qddd = 0;
else if (t >= Ta - Tj1 && t < Ta)
qddd = -jmin;
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qddd = 0;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qddd = -jmax;
else if (t >= T - Td + Tj2 && t < T - Tj2)
qddd = 0;
else if (t >= T - Tj2 && t <= T)
qddd = jmax;
return qddd;
}
/* Headfile.h */
#ifndef _Headfile_H
#define _Headfile_H
#include "stm32f4xx.h"
#include "./can/can1.h"
#include "./systick/systick.h"
#include "math.h"
#include "stdio.h"
#include "stdint.h"
#include "misc.h"
#include "./usart/usart.h"
#include "stdlib.h"
#include "./key/key.h"
#include "./scurve/scurve.h"
#endif
这里我只是做了个简单的小实验，验证一下自写的s曲线c代码，然后接通了一下robomodule驱动器和STM32F429之间的CAN通讯，没写定时程序（其实我是不会写……），让电机跑了下位置曲线，大致看了下速度曲线，发现emmmm还可以，就……结束了……
所以这篇相当于未完待续吧，等啥时候我琢磨一下定时脉冲啥的再回头完善完善。
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
15
0- 0
已为社区贡献4条内容
所有评论(0)