斯图尔特平台研究一：自平衡代码

从网上找到的几个例子来看（以舵机控制平台为例），自平衡的核心程序就是这一个函数

x = map(num,fromMAX,fromMIN,toMAX,forMIN);

首先看一下它的参数：map(num，旧区间初值，旧区间终值，新区间初值，新区间终值)；意思就是把num这个数从旧区间映射到新区间，就是高中数学知识那个映射，而且是最简单的线性映射。

我们的初始区间可以理解为舵机的初始模拟值范围，这个根据舵机摆放的位置不同，需要实地测量出它的转动模拟值范围。需要注意的是，根据供电功率的不同，模拟值范围会变化。

例如我们找到了六个舵机的12个范围值。

int floors  [6] = { 237, 115, 231, 111, 112, 109 };
int ceilings[6] = { 492, 358, 502, 362, 351, 356 };

接下来我们需要我们的新区间，新区间范围比较固定，根据你的舵机范围和安装角度来定，可以是0~180度，也可以表示为模拟值0~1024。同理，可以设定为更小角度10~170度。

最后map里的第一个参数map就是analogRead读取的模拟值。经过不断的读取模拟值——得出平台的倾斜角度——给舵机一个反向倾斜角度，就可以达到自平衡的效果。

我们发现读取的是瞬时模拟值，读取完后平台还在继续倾斜，如果直接把瞬时值带入，就会有误差，达不到平衡。怎么处理呢。

我们把得到的六个舵机模拟值叫做当前位置值pos[NUM_MOTORS]，把先前的移动位置差值叫previous_diff[NUM_MOTORS]，把期望得到的位置值叫desired_pos[NUM_MOTORS]，每次位移的差值之和叫total_diff[NUM_MOTORS]。并且设定两个增量值，当前增量current_inst[NUM_MOTORS]和先前增量previous_inst[NUM_MOTORS]。

// Position variables
int16_t pos[NUM_MOTORS];            // current position (measured by analog read) of each actuator
int16_t input[NUM_MOTORS];          // intermediate input retrieved from the serial buffer
uint16_t desired_pos[NUM_MOTORS];   // desired (user-inputted and validated) position of each actuator// Feedback variables
int16_t pos_diff;                   // difference between current and desired position
int16_t prop_diff;                  // position difference used for proportional gain
int16_t previous_diff[NUM_MOTORS];  // last position difference for each actuator; for derivative gain
int16_t total_diff[NUM_MOTORS];     // cumulative position difference for each actuator; for integral gain
uint8_t previous_inst[NUM_MOTORS];  // starting/past sample instance for each actuator; for derivative gain
uint8_t current_inst[NUM_MOTORS];   // new/incrementing sample instance for each actuator; for derivative gain

可以看到，desired_pos[NUM_MOTORS]是变量，需要我们人为输入，目前自动平衡平台不做考虑。pos[NUM_MOTORS]就是我们经过map函数得到的量。

令pos_diff = pos[motor]，当位置量太小（比如说模拟值只增加了1，未超过运动阈值），说明平台运动的还不明显，可视为误差忽略。如果大于阈值，计入总变量total_diff[NUM_MOTORS]。

当previous_diff[motor] != pos_diff，也就是说，舵机倾斜的角度和你保持平衡需要的角度不一样了，说明模拟值只读了那一个时刻的值，没有读下一时刻的值，下一时刻就没法预料了，我们就给舵机加一个增量，来代替那个没法预料的值。

// Compute the error valuepos_diff = pos[motor] - desired_pos[motor];// Compute error-dependent PID variablesif (abs(pos_diff) <= POS_THRESHOLD[motor]){prop_diff = 0;total_diff[motor] = 0;}else{prop_diff = pos_diff;total_diff[motor] += pos_diff;}if (previous_diff[motor] != pos_diff){previous_inst[motor] = current_inst[motor];current_inst[motor] = 1;}

然后根据矫正关系式来矫正位置。

typedef enum _MotorDirection  // to clarify the direction in which actuators move
{RETRACT = 0,EXTEND  = 1
} MotorDirection;// PID feedback parameters
const uint8_t POS_THRESHOLD[NUM_MOTORS] = { 4, 4, 4, 4, 4, 4 };
const float P_COEFF[NUM_MOTORS]         = { 3, 3, 3, 3, 3, 3 };
const float I_COEFF[NUM_MOTORS]         = { 0.03, 0.035, 0.03, 0.03, 0.03, 0.035 };
const float D_COEFF[NUM_MOTORS]         = { 0.025, 0.025, 0.025, 0.025, 0.025, 0.025 };

 // Compute PID gain valuesp_corr = P_COEFF[motor] * prop_diff;i_corr = I_COEFF[motor] * total_diff[motor];d_corr = D_COEFF[motor] * ((float) pos_diff - previous_diff[motor]) / (current_inst[motor] + previous_inst[motor]);// Compute the correction value and set the direction and PWM for the actuatorcorr = p_corr + i_corr + d_corr;dir[motor] = (corr > 0) ? RETRACT : EXTEND;  // direction based on the sign of the errorpwm[motor] = constrain(abs(corr), MIN_PWM, MAX_PWM);  // bound the correction by the PWM limits// Move the actuator with the new direction and PWM valuesdigitalWrite(DIR_PINS[motor], dir[motor]);analogWrite(PWM_PINS[motor], pwm[motor]);// Increment sample-dependent PID variables for the next sampling timecurrent_inst[motor] += 1;previous_diff[motor] = pos_diff;

当然，矫正关系式有很多种，这里给出所参考代码的一种。代码来源：https://github.com/progressiveautomations/Stewart-Platform

斯图尔特平台研究一：自平衡代码相关推荐

生物统计师与临床医生协同研究使用的低代码洞察平台丨数据科学 x 临床医学
临床研究的"多角色"特性临床研究是以疾病的诊断.治疗.预后和病因为主要研究内容,以患者为主要研究对象的科学研究活动.现代临床研究项目的开展具有"多角色"参与的 ...
低代码开发平台_什么是低代码和无代码开发平台？
低代码/无代码开发平台是一种可视化软件开发环境,允许公民开发人员拖放应用程序组件,将它们连接在一起并创建移动或Web应用程序.这种模块化方法使专业开发人员可以免除他们逐行编写代码的需要,从而更快地构建 ...
烟草物流信息综合管理平台研究与应用——以H市卷烟厂烟草物流配送中心为例
烟草物流信息综合管理平台研究与应用 --以H市卷烟厂烟草物流配送中心摘要自我国正式加入世界贸易组织(WTO)以来,我国卷烟与外国卷烟之间对零售终端市场的竞争日益激烈,中国烟草目前正面临前所未有的 ...
当今自媒体广告平台研究参考分析
当今自媒体广告平台研究参考有哪些?当今社会自媒体平台可谓大行其道,成为互联网中主流趋势,他不仅匹配度高,传播效果好,还具备较多的商业价值.那对于其中的工作人员来说,自媒体广告平台研究参考有哪些呢? 1 ...
微信第三方平台研究总结
微信第三方平台研究总结一.背景客户反馈小程序更新不及时导致用户体验不好客户不想自己审核发布一系列麻烦操作现阶段小程序采用的是微信ci工具文档,通过Jenkins打包后使用命令上传到微信指定版本 ...
DLL内线程同步主线程研究（子线程代码放到主线程执行）
DLL内线程同步主线程研究(子线程代码放到主线程执行) 我们在实际项目中经常会用到多线程编程,比如Socket编程等,在创建的线程内同步主线程一般使用Synchronize方法实现子线程操作放到主线程 ...
使用abapGit在ABAP On-Premises系统和SAP云平台ABAP环境之间进行代码传输
SAP ABAP顾问朋友们,应该都使用过SAPLink这个工具.如果两个ABAP Netweaver系统没有建立起传输路径时,我们无法使用标准的SE10事务码创建传输请求的方式进行这两个系统间的代码传 ...
python自动化测试平台方案_基于Python的软件测试自动化平台研究
基于 Python 的软件测试自动化平台研究沈啸 [摘要] 摘要: 21 世纪是计算机和网络技术高速发展的时代,目前我国的软件行业开发程度明显落后于欧美等发达国家,同时相对于开发工作而言,软件 ...
青少年编程教育平台后台—登录注册（代码编写）
青少年编程教育平台后台-登录注册(代码编写) 一.新建项目二.编写配置文件 (一)pom.xml配置文件 <?xml version="1.0" encoding=&quo ...

斯图尔特平台研究一：自平衡代码

斯图尔特平台研究一：自平衡代码相关推荐

最新文章

热门文章