8547制作Segway

非常乐

8547  Segway

http://www.nxtprograms.com/programs2/Segway.zip官方网站地址。

没有试验成功，只好把程序研究了一下，知道他是怎么工作的了，但是不知道为什么这样计算才能站立，力学计算？还是不断测试？

给定三个变量。KP =25，KI =1，Kd=10。然后马达A停止，马达A驱动的是机器人的“身子”和“脑袋”。

三个循环，每次执行0.2秒的G声和0.8秒的等待。

光电传感器读取数值到Mid变量。然后0.2秒C声。

设置两个变量PrevError=0，Integral=0.

Error=颜色传感器-Mid    颜色传感器与初始的变化值

Integral=Integral + Error   颜色传感器变化累计值

Derivative=Error-PrevError    变化值-0，由于后面Error赋值给PrevError，相当于变化值减前一次变化值

KD* Derivative+ Integral*KI            10*（变化值-变化值）+  变化累计值*1

PrevError=Error                     Error赋值给PrevError

KP*Error+ KD* Derivative+ Integral*KI      25*变化值+10*（变化值-变化值）+ 变化累计值*1

上值与0比较 大于0电机前进

上值还为电机功率值

上值如果在-1000到1000之间，循环，否则不循环。

直立时，颜色传感器返回的值是多少呢。修改程序，电机停止运转，其他不变。便于采集数据。

采集颜色传感器中红色光线传感器。

此时传感器到桌面的实际搞定了约14-15mm

再次测量一次

仔细发现，机器人前倾数据变大，机器人后仰数据变小。颜色传感器靠近数值变大，远离数值变小。

现在把数值带入程序看看

//-----------------------------------------------------------------------------

Error=颜色传感器-Mid    颜色传感器与初始的变化值

Integral=Integral + Error   颜色传感器变化累计值

Derivative=Error-PrevError    变化值-0，由于后面Error赋值给PrevError，相当于变化值减前一次变化值

                           我理解是变化速率

KD* Derivative+ Integral*KI            10*（变化值-变化值）+  变化累计值*1

PrevError=Error                     Error赋值给PrevError

KP*Error+ KD* Derivative+ Integral*KI      25*变化值+10*（变化值-变化值）+ 变化累计值*1

上值与0比较 大于0电机前进，小于0电机后退，

上值还为电机功率值

上值如果在-1000到1000之间，循环，否则不循环。

//----------------------------------------------------------------------------

假如前倾：传感器从最初47变为48

Error=48-47=1

Integral=1

Derivative=1

PrevError=1

KP*Error+ KD* Derivative+ Integral*KI=25*1+10*1+1*1=36

马达功率36 前进

若再次前倾，传感器变为50

Error=50-47=3

Integral=1+3=4

Derivative=3-1=2

PrevError=3

KP*Error+ KD* Derivative+ Integral*KI=25*3+10*2+4*1=99

马达功率99前进

若机器人开始后仰

同理传感器一下变为48

Error=48-47=1

Integral=4+1=5

Derivative=1-3=-2

PrevError=1

KP*Error+ KD* Derivative+ Integral*KI=25*1+10*(-2）+5*1=10

此时仍以10前进。

再变为47

Error=0

Integral=5

Derivative=0-1=-1

PrevError=0

KP*Error+ KD* Derivative+ Integral*KI=25*0+10*(-1）+5*1=-5

功率5后退，因为此时机器人趋势是往后仰，虽然直立姿势，但仍需以功率5后退。

…………

可知，KP*Error+ KD* Derivative+ Integral*KI公式中

KP值为25，所以Error影响最大。即与原始位置的差异值最影响机器人前后速度

KD为10，Derivative影响其次，即变化速率次要影响机器人前后速度

KI为1，Integral即变化累计值影响不大，对机器人前后运动修正。

具体为何这种计算方式会让机器人稳定，不得而知，应该不会是力学运算出来的吧，那太神了。应该是多次调试的结果吧。

呜呜，我的多次都没有站起来，可惜。

附：

从0-20cm再次测量，中间第二个波峰，是因为又靠近了一些。

发现距离为0时，返回13左右，在一段距离之后迅速变大，然后又变小，最后保持在50之内。

应该是1cm到10cm左右是测量敏感区，以后搭建时，这个传感器的测量范围要控制在这个距离内了。

机器人程序二：SegwayBT

给定3个变量，Kp=40  Ki=1.5  Kd=15, 然后马达A停止，马达A驱动的是机器人的“身子”和“脑袋”。

三个循环，每次执行0.2秒的G声和0.8秒的等待。

读取光电传感器红光赋予Mid ，0.2秒C声

三个变量 PrevError=0，Integral=0 ，LeanAngle=0

不仅三个系数不一致，而且多定义一个变量，继续看。

蓝牙，接收逻辑信息

是否收到信息发给第一个判断，收到真或伪信息发给第二个判断。

若收到信息，且为真，则

LeanAngle =LeanAngle-3        收到蓝牙1信号，LeanAngle减3

若收到信息，且为伪，则

LeanAngle =LeanAngle+3        收到蓝牙0信号，LeanAngle加3

若接收不到信息，则不运行任何程序。

（LeanAngle-A角度）*10>0  则A电机以此功率前进。

Error=光线传感器-Mid             传感器变化值

Integral=Integral + Error             传感器变化累计值

Derivative=Error-PrevError    变化值-0，由于后面Error赋值给PrevError，相当于变化值减前一次变化值

Derivative*Kd+ Integral*Ki

PrevError=Error                   Error赋值给PrevError

Derivative*Kd+ Integral*Ki+Error*Kp

若上值>0 , B C 马达以此值为功率前进。

上值在-1000到1000范围内，从蓝牙接收之前循环

蓝牙部分主要是1信号是，身子前倾，0信号，身子后仰，从而影响机器人中心，变化光电传感器数值。

其他部分与第一个程序基本一样，主要差别在3个参数

第一个  KP*Error+ KD* Derivative+ Integral*KI

与原始位置的差异*25+变化幅度*10+变化累计*1

第二个 Derivative*Kd+ Integral*Ki+Error*Kp

变化幅度*15+变化累计*1.5+与原始差异*40

减小变化幅度的影响，增加净变化值与变化累计值的影响，不知道为什么这样就好使了，呜呜，试过几遍都好使，看来，主要是调整这三个系数。可以找到满足平衡的状态。

看了一篇PID文章，有以下总结，暂且记下，后面再学习。
整定参数寻最佳,从小到大逐步查;
先调比例后积分,微分作用最后加;
曲线震荡很频繁,比例刻度要放大;
曲线漂浮波动大,比例刻度要拉小;
曲线偏离回复慢,积分时间往小降;
曲线波动周期长,积分时间要加长;
曲线震荡动作繁,微分时间要加长.



视频地址：
http://v.youku.com/v_show/id_XNDIyNjczNTEy.html

非常乐

呜呜，把第二个程序SegwayBT下载上去了，发现，他居然立住了，好吧，在看看第二个程序和第一个有什么差异。

langao1984

大开眼界，学习学习！

lsm910817

好复杂  自己还差得很远啊

变形金刚

好牛，好复杂

狙击手

用处很大

clx2012

喜欢这种程序讨论贴