不稳定的磁悬浮

蓝天-彭

ahha007 发表于 2014-8-22 21:30
教我们大家一下怎么搞的，能否出个攻略？

大家可以参考盗梦陀螺那个攻略，我也是按照他做的
http://www.diy-robots.com/?page_id=685

smileFei

这也太高大上了吧。。。

蓝天-彭

下面说一下主要事项：
1、线圈的接线：接线方面，互相对面的一组线圈之间反相连接。也就是说当在一对线圈两端通电时，一个会对浮子产生斥力，另一个会产生引力，正好是相反的，连推带拉才給力！
2、霍尔传感器接线：传感器的位置尽量在线圈中间，高度上也尽量放在中心的高度

3、Android UNO一共有四种IO接口：模拟输入输出和数字输入输出。其中模拟输入标记为“ANALOG IN”，可以测量0~5V的电压，对应在代码中的读数范围是0~1023，示例代码如下：
int readValue1 = analogRead(read1Pin);  
模拟输出实际上输出的是一串方波，通过高低电压的占空比来产生“平均电压”。在板上对应的标记是PWM，输出电压同样是0~5V，但是请注意设置的数值范围却是0~255。示例代码如下：
analogWrite(power1Pin, Pid1.power);  
数字输入输出需要先设置管脚的模式，直接看示例代码吧，相信聪明的你肯定明白：
1.pinMode(Pin1, OUTPUT);     //设置为输出管脚  
2.pinMode(Pin2, INPUT);      //设置为输入管脚  
3.digitalWrite(Pin1, HIGH);  //输出高电压  
4.int v = digitalRead(Pin2); //读取Pin2的电压，返回结果是0或1  

接口2-13所有的接口都可以作为数字输入输出接口，而其中只有3、5、6、9、10、11、可以用作PWM模拟输出，传感器和电位器的读数显然要用模拟输入，而线圈电流的控制也显然要用模拟输出。强烈建议把接线的编号集中写在程序的最前面，这样可以一目了然的看出是怎么接的线：
int adjust1Pin = A2;    //用来调节A的电位器  
int adjust2Pin = A3;    //用来调节B的电位器  
int read1Pin = A4;      //用来连接输入A传感器  
int read2Pin = A5;      //用来连接输入B传感器  
int i1Pin = 7;        //连接电机驱动板的I1接口  
int i2Pin = 8;        //连接电机驱动板的I2接口  
int i3Pin = 12;        //连接电机驱动板的I3接口  
int i4Pin = 13;        //连接电机驱动板的I4接口  
int power1Pin = 6;     //连接电机驱动板的EA接口  
int power2Pin = 5;     //连接电机驱动板的EB接口

细心的朋友一定看到上面代码中，有I1到I4四个接口，我将会把它们设置成数字输出。这里再顺便介绍下L298N的用法。
L298N直接连接了20V的电源，通过板内取电的方式提供5V电压给电路使用。板上包含了对称的两组电流驱动电路，以I1,I2,EA为例：
1.I1=0;I2=1;  //输出正电压，EA范围0~255时，输出电压对应为0~+20V  
2.I1=1;I2=0;  //输出负电压，EA范围0~255时，输出电压对应为0~-20V  
3.I1=0;I2=0;  //输出电压均为0  
4.I1=1;I2=1;  //输出电压均为0  
我们可以用数字输出I1和I2控制线圈的电压方向，用模拟输出EA控制电压的大小。I3，I4和EB是完全一样的，这里就不多说啦。友情提醒一下，Arduino的地线，L298N的地线，还有焊接电路的地线，这些地线一定要都连在一起。

蓝天-彭

进入正题，介绍提了多次的PID平衡算法。先从网上摘抄一段：
      当今的自动控制技术都是基于反馈的概念。反馈理论的要素包括三个部分：测量、比较和执行。测量关心的变量，与期望值相比较，用这个误差纠正调节控制系统的响应。
      这个理论和应用自动控制的关键是，做出正确的测量和比较后，如何才能更好地纠正系统。
在工程实际中，应用最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制，又称PID调节。它以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或不能通过有效的测量手段来获得系统参数时，最适合用PID控制技术。PID控制，实际中也有PI和PD控制。PID控制器就是根据系统的误差，利用比例、积分、微分计算出控制量进行控制的。
比例（P）控制
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。
积分（I）控制
      在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。
微分（D）控制
     在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后(delay)组件，具有抑制误差的作用，其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入 “比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分(PD)控制器能改善系统在调节过程中的动态特性。
看概念可能有点晕，举个小小的例子也许能帮助理解。

      假设我们想把一个小球稳定在一个光滑的坡顶，这显然是一个不平衡的系统，稍有扰动小球就会滚下来。假设恰好平衡的位置坐标是L，我们可以测量到小球的位置是x，那么怎么给小球施加f(x)的力反馈，让它能够平衡呢？
最直观的想法就是f(x) = Kp*(L-x)，简单的说就是你在左边我就向右推，你在右边我就向左推，这就是比例因子P；
现在考虑两种情况，同样是在x位置，小球静止和小球具有速度V这两种情况。很明显，如果V>0，我们只需要施加更小的力，因为小球自身的惯性会让它运动向平衡位置。所以可以修正f(x) = Kp*(L-x) – Kd*V。因为速度一般不容易测量，我们常常用位置的变化Δx除以测量的时间差Δt来计算速度，所以这就是微分因子D；
情况继续发生变化，上面考虑的是斜坡静止的情况，如果这个变态的斜坡是移动的怎么办呢？（例如两轮平衡机器人实际上是可以运动的，对于静止的磁悬浮来说，不需要考虑这个参数）这时候我们需要不断的累加并平均x值，来计算平衡位置的L，这个就是积分因子I；
       以上就是PID的简要介绍。说起来容易，真正调试的时候，最恼火的就是这几个参数到底是多少，办法只有一个：试，不断的试！
       当然，试验也不要当老黄牛，累死都没人知道。我曾经试其中某个参数，从0.1开始，每次加0.01，差点试到崩溃。后来想了个办法，用串口把Arduino的读数发送到电脑，然后用软件分析结果，看到数据明显发现这个值偏小，发狠改到20，就这样成功了…..

蓝天-彭

源码：
int adjust1Pin = A2;    //用来调节A的电位器  
int adjust2Pin = A3;    //用来调节B的电位器  
int read1Pin = A4;      //用来连接输入A传感器  
int read2Pin = A5;      //用来连接输入B传感器  
int i1Pin = 7;        //连接电机驱动板的I1接口  
int i2Pin = 8;        //连接电机驱动板的I2接口  
int i3Pin = 12;        //连接电机驱动板的I3接口  
int i4Pin = 13;        //连接电机驱动板的I4接口  
int power1Pin = 6;     //连接电机驱动板的EA接口  
int power2Pin = 5;     //连接电机驱动板的EB接口  
  boolean debug = true;  
boolean writeLog = true;  
double setKd1 = 30;  
double setKd2 = 30;  
double setKp = 4;  
int offset = 80;  
int delayMs = 1;  
int tick = 0;  
int myLog[3500];  
typedef struct {  
  double target;  
  double aver;  
  double Kp;  
  double Kd;  
  int preError;  
  int power;  
  boolean flag;  
  double v;  
} PID;  
  
PID Pid1, Pid2;  
  
void setup()  
{  
  pinMode(i1Pin, OUTPUT);     //I1和I2都是数字信号  
  pinMode(i2Pin, OUTPUT);     //通过设置I1和I2来控制电流方向  
  pinMode(i3Pin, OUTPUT);     //I1和I2都是数字信号  
  pinMode(i4Pin, OUTPUT);     //通过设置I1和I2来控制电流方向  
  pinMode(power1Pin, OUTPUT);  //按占空比方式输出的模拟信号  
  pinMode(power2Pin, OUTPUT);  //按占空比方式输出的模拟信号  
  Serial.begin(9600);          //设置波特率  
  TCCR0B = 0x01;   // Timer 0: PWM 5 &  6 @ 16 kHz  
  TCCR1B = 0x01;   // Timer 1: PWM 9 & 10 @ 32 kHz  
  TCCR2B = 0x01;   // Timer 2: PWM 3 & 11 @ 32 kHz  
  Pid1.Kp = setKp;  
  Pid1.preError = 0;  
  Pid1.Kd = setKd1;  
  Pid1.power = 0;  
  Pid1.flag = true;  
  Pid1.target = 300;  
  Pid1.aver = 0;  
  Pid1.v = 0;  
  Pid2.Kp = setKp;  
  Pid2.preError = 0;  
  Pid2.Kd = setKd2;  
  Pid2.power = 0;  
  Pid2.flag = true;  
  Pid2.target = 300;  
  Pid2.aver = 0;  
  Pid2.v = 0;  
  tick = 0;  
}  
  
int tick2 = 0;  
//boolean rotateFlag = true;  
void loop()  
{  
  
  //=======第一组电位器和传感器========  
  int readValue1 = 0;  
  for(int i = 0; i < 4; i++) readValue1 += analogRead(read1Pin);  
  readValue1 >>= 2;  
  //readValue1 += (Pid1.flag ? 1 : -1) * Pid1.power / 17;  
  int adjustValue1 = analogRead(adjust1Pin); //410 analogRead(adjust1Pin);  
  Pid1.aver = Pid1.aver * 0.9995 + readValue1 * 0.0005;  
  Pid1.target = Pid1.target + (Pid1.target - Pid1.aver) / 100.0;  
  Pid1.target = max(0, max(adjustValue1 - offset, Pid1.target));  
  Pid1.target = min(755, min(adjustValue1 + offset, Pid1.target));  
  
  //=======第二组电位器和传感器=======  
  int readValue2 = 0;  
  for(int i = 0; i < 4; i++) readValue2 += analogRead(read2Pin);  
  readValue2 >>= 2;  
  //readValue2 += (Pid2.flag ? 1 : -1) * Pid2.power / 6;  
  int adjustValue2 = analogRead(adjust2Pin); //240 analogRead(adjust2Pin);  
  Pid2.aver = Pid2.aver * 0.9995 + readValue2 * 0.0005;  
  Pid2.target = Pid2.target + (Pid2.target - Pid2.aver) / 1000.0;  
  Pid2.target = max(0, max(adjustValue2 - offset, Pid2.target));  
  Pid2.target = min(755, min(adjustValue2 + offset, Pid2.target));  

  //Calculate power values  
  double v, error;  
  error = readValue1 - Pid1.target;  
  v = error - Pid1.preError;  
  Pid1.v = (Pid1.v * 6 + v) / 7;  
  Pid1.power = (int)error * Pid1.Kp + Pid1.v * Pid1.Kd;  
  Pid1.flag = Pid1.power > 0;  
  Pid1.power = abs(Pid1.power);  
  if(Pid1.power>255) Pid1.power = 255;  
  Pid1.preError = error;  
  
  error = readValue2 - Pid2.target;  
  v = error - Pid2.preError;  
  Pid2.v = (Pid2.v * 6 + v) / 7;  
  Pid2.power = (int)error * Pid2.Kp + Pid2.v * Pid2.Kd;  
  Pid2.flag = Pid2.power < 0;  
  Pid2.power = abs(Pid2.power);  
  if(Pid2.power>255) Pid2.power = 255;  
  Pid2.preError = error;  
  
  //Write PMW to control the floa  
  digitalWrite(i1Pin, Pid1.flag);  
  digitalWrite(i2Pin, !Pid1.flag);  
  analogWrite(power1Pin, Pid1.power);  
  
  digitalWrite(i3Pin, Pid2.flag);  
  digitalWrite(i4Pin, !Pid2.flag);  
  analogWrite(power2Pin, Pid2.power);  

// Serial.println(adjustValue1);  
// Serial.println(adjustValue2);  
  //Serial.println(readValue1);  
  //Serial.println(readValue2);  
  //delay(32000);  
}  
最后几行是利用串口查看电位器和霍尔传感器的读数。可以取消注释来查看，注意实际实验时要注释掉这几行，不然有延时，浮子是不稳定的