Arduino上拉磁悬浮

johnsonzzd

折腾了几天终于把这个东西搞定了。看似简单，还是有一些小技巧的。
主要材料：

• Arduino UNO主控
• SS495线性霍尔采集位置信息
• L298N驱动
• 直径12.7mm强磁球
• 电磁铁12V，线圈电阻10欧
  

主要参数：PID控制，采样周期1ms，PWM频率 3921Hz，悬浮距离30mm，电流300mA
筷子版的，下面是一个磁体，上面是一个小铁块，中间是一截筷子。悬浮距离比较小，但是方便进行建模和控制理论分析。

磁球版，悬浮距离很大，不容易进行理论分析。

1. const int SS495_PIN = A0;  // Analog input pin that the SS495 is attached to
   
2. const int PWM_PIN=3;  // Pins 3 and 11 are connected to Timer 2.
   
3. 
   
4. const int SampleTime=600;
   
5. const int PWM_BIAS=128;
   
6. 
   
7. int Setpoint=850;
   
8. double Kp=1, Ki=0.001, Kd=50;
   
9. 
   
10. void setupCoilPWM()
    
11. {  
    
12.    // Setup the timer 2 as Phase Correct PWM, 3921 Hz.
    
13.    pinMode(3, OUTPUT);
    
14.    // Timer 2 register: WGM20 sets PWM phase correct mode, COM2x1 sets the PWM out to channels A and B.
    
15.    TCCR2A = 0;
    
16.    TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
    
17.    // Set the prescaler to 8, the PWM freq is 16MHz/255/2/<prescaler>
    
18.    TCCR2B = 0;
    
19.    TCCR2B = _BV(CS21);
    
20. }
    
21. 
    
22. void writeCoilPWM(uint8_t pin, int val)
    
23. {
    
24.       OCR2B = val;
    
25. }
    
26. 
    
27. void PID_Compute( )
    
28. {
    
29.   static int lastError=0;
    
30.   static unsigned long int lastTime=0;
    
31.   static double iiterm=0;
    
32. 
    
33.   unsigned long now,timeChange;
    
34.   int input,output,error,piterm,diterm;
    
35. 
    
36.   now = micros();
    
37.   timeChange = (now - lastTime);
    
38.   if(timeChange>=SampleTime)
    
39.   {
    
40.     lastTime = now;
    
41. 
    
42.     input=read_input();
    
43. 
    
44.     error = -(Setpoint - input);
    
45. 
    
46.     iiterm+= (Ki * error);
    
47.     iiterm=constrain(iiterm,-255,255);
    
48. 
    
49.     piterm=Kp * error;
    
50.     diterm=Kd * (error - lastError)*1000/timeChange;
    
51.     output = PWM_BIAS+(piterm + diterm);
    
52.     output=constrain(output,0,255);
    
53.     writeCoilPWM(PWM_PIN, output);
    
54. 
    
55.     // print the results to the serial monitor:
    
56.     //sprintf(str,"%04d,%+04d,%+04d,%+04d,%04d",error,piterm,diterm,(int)iiterm,output);
    
57.     //Serial.println(str);   
    
58. 
    
59.     /*Remember some variables for next time*/
    
60.     lastError = error;
    
61.    }
    
62. }
    
63. 
    
64. int read_input()
    
65. {
    
66.   static int last=0;
    
67.   int r;
    
68.   r=analogRead(SS495_PIN);
    
69.   if(abs(r-last)<4)
    
70.     r=last;
    
71.   else
    
72.     last=r;
    
73.   return r;
    
74. }
    
75. 
    
76. void setup() {
    
77.   setupCoilPWM();
    
78. }
    
79. 
    
80. void loop() {
    
81.   // read the analog in value:
    
82.   PID_Compute();
    
83. }
    

复制代码

下一步准备加上串口通讯，用matlab做上位机界面。已经做好了Arduino的自平衡车和两旋翼模型，有空也放上来。

自平衡车就在这里：
http://www.geek-workshop.com/thread-2398-1-1.html

单旋翼实验装置，已经发到amobbs了，有详细资料：
http://www.amobbs.com/thread-5505843-1-1.html

wing

非常有趣的东西 , 代码先收下了,有空慢慢看

darkorigin

很久以来一直在泡一个blog，跟大家分享下  动力老男孩的博客 （为免广告嫌疑，请自行百度。）
他用了多个电磁线圈实现了 陀螺（自制磁铁陀螺）的悬浮自稳（当然，Arduino的PID平衡算法和霍尔元件也功不可没）
蛮有意思。

一直以来也学了不少。

darkorigin

第一幅图用皮筋捆着 上面亮灯的板子能透露细节吗？谢谢！

arduino-tinker

darkorigin 发表于 2012-5-21 00:56
第一幅图用皮筋捆着 上面亮灯的板子能透露细节吗？谢谢！

那应该是L298N电极驱动板吧~~看楼主的电磁铁是12V的，Arduino提供不了那么大的电压。Arduino先连L298N，外部供电给L298N，再连电磁铁。Arduino通过PWM控制L298N，间接控制电磁铁。
以上纯属猜测，望指点。

黑马

调参数的话悬浮高度范围有多大？

Amuro_zhou

我刚要说，是不是仿照动力老男孩做的，就看到他的名字了……
话说他的磁悬浮鸡蛋还在我们这里

johnsonzzd

那个带个皮筋的板子是L298驱动板，在这里有点浪费，用个功率三极管或者MOS管就可以了。
这个东西的高度调整范围主要受限于霍尔元件。SS495从0到饱和，磁球的运动范围只有2、3毫米。所以有用图像识别的手段测量高度的，调整范围就很大了。
我看过动力老男孩的东西，受到启发，确实不错，但是有一个缺点是理论分析不够。磁悬浮不是什么创新的东西，国内的项目和国外的比起来还有很大差距。有兴趣的可以参考下面这个，从实践到理论非常详细。
http://www.coilgun.info/levitation/home.htm

johnsonzzd

完整版，用串口向PC机发送信息：

1. const int SS495_PIN = A0;  // Analog input pin that the SS495 is attached to
   
2. const int PWM_PIN=3;  // Pins 3 and 11 are connected to Timer 2.
   
3. 
   
4. const double SampleTime=0.5;  //ms
   
5. const int PWM_BIAS=128;
   
6. 
   
7. int Setpoint=850;
   
8. double Kp=1, Ki=0.01, Kd=90;
   
9. double piterm,diterm, iiterm=0;
   
10. int input,output,error;
    
11. unsigned long now,timeChange;
    
12. 
    
13. void setupCoilPWM()
    
14. {  
    
15.   // Setup the timer 2 as Phase Correct PWM, 3921 Hz.
    
16.   pinMode(3, OUTPUT);
    
17.   // Timer 2 register: WGM20 sets PWM phase correct mode, COM2x1 sets the PWM out to channels A and B.
    
18.   TCCR2A = 0;
    
19.   TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
    
20.   // Set the prescaler to 8, the PWM freq is 16MHz/255/2/<prescaler>
    
21.   TCCR2B = 0;
    
22.   TCCR2B = _BV(CS21);
    
23. }
    
24. 
    
25. void setupControlTimer()
    
26. {  
    
27.   //set timer1 in CTC mode.
    
28.   //"ATmega4_88_168_328_DataSheet" , section 16.9.2 Clear Timer on Compare Match (CTC) Mode
    
29.   cli();          // disable global interrupts
    
30.   TCCR1A = 0;     // set entire TCCR1A register to 0
    
31.   TCCR1B = 0;     // same for TCCR1B
    
32. 
    
33.   // turn on CTC mode:
    
34.   TCCR1B |= (1 << WGM12);
    
35.   // Set CS12 bits for 256 prescaler:
    
36.   TCCR1B |= (1 << CS12);
    
37.   // set compare match register to desired timer count:
    
38.   OCR1A = (SampleTime/1000)*(16000000/256);
    
39.   // enable timer compare interrupt:
    
40.   TIMSK1 |= (1 << OCIE1A);
    
41.   sei();          // enable global interrupts
    
42. }
    
43. 
    
44. void writeCoilPWM(uint8_t pin, int val)
    
45. {
    
46.     OCR2B = val;
    
47. }
    
48. 
    
49. ISR(TIMER1_COMPA_vect)
    
50. {
    
51.   PID_Compute();
    
52. }
    
53. 
    
54. void PID_Compute( )
    
55. {
    
56.   static int lastError=0;
    
57.   static unsigned long int lastTime=0;
    
58. 
    
59.   now = micros();
    
60.   timeChange = (now - lastTime);
    
61.   if(timeChange>=SampleTime)
    
62.   {
    
63.     lastTime = now;
    
64. 
    
65.     input=read_input();
    
66. 
    
67.     error = -(Setpoint - input);
    
68. 
    
69.     iiterm+= (Ki * error);
    
70.     iiterm=constrain(iiterm,-255,255);
    
71. 
    
72.     piterm=Kp * error;
    
73.     diterm=Kd * (error - lastError)/SampleTime;
    
74.     output = PWM_BIAS+(piterm + diterm);
    
75.     output=constrain(output,0,255);
    
76.     writeCoilPWM(PWM_PIN, output);
    
77. 
    
78.      /*Remember some variables for next time*/
    
79.     lastError = error;
    
80.   }
    
81. }
    
82. 
    
83. int read_input()
    
84. {
    
85.   static int last=0;
    
86.   int r;
    
87.   r=analogRead(SS495_PIN);
    
88.   if(abs(r-last)<4)
    
89.     r=last;
    
90.   else
    
91.     last=r;
    
92.   return r;
    
93. }
    
94. 
    
95. void setup() {
    
96.   setupCoilPWM();
    
97.   setupControlTimer();
    
98.   // initialize serial communications at 9600 bps:
    
99.   Serial.begin(115200);
    
100. }
     
101. 
     
102. void loop() {
     
103.   // read the analog in value:
     
104.   // User commands.
     
105.   //   if( Serial.available() )
     
106.   //   {
     
107.   //      processCommand( Serial.read() );
     
108.   //   }
     
109.      // print the results to the serial monitor:
     
110.   char str[50];
     
111.   sprintf(str,"%ld, %03d, %04d, %+04d, %+04d, %+04d, %04d",now/1000, input,error,piterm,diterm,(int)iiterm,output);
     
112.   Serial.println(str);   
     
113. 
     
114.   delay(20);
     
115. }
     

复制代码

PC机监控程序。Matlab，能够自动滚屏

1. s1=serial('COM4','Baudrate',115200);
   
2. fopen(s1);
   
3. x=0;time=0;input=0;error=0;piterm=0;diterm=0;iiterm=0;pwm=0;
   
4. str='';
   
5. sen=0;
   
6. 
   
7. 
   
8. try                             % use try catch to ensure fclose
   
9. figure(1)
   
10. hErr=plot(x,error);
    
11. %title('Dados de LDR Aquisitados');
    
12. hold all
    
13. hP=plot(x,piterm);
    
14. hD=plot(x,diterm);
    
15. hPWM=plot(x,pwm);
    
16. 
    
17. j=1;
    
18. while(j<2000)
    
19.     str=fscanf(s1);
    
20.     sen=str2num(str);
    
21.     if length(sen)<7
    
22.         continue
    
23.     end
    
24.    
    
25.     x(j)=j
    
26.    time(j)=sen(1);
    
27.    input(j)=sen(2);
    
28.    error(j)=sen(3);
    
29.    piterm(j)=sen(4);
    
30.    diterm(j)=sen(5);
    
31.    iiterm(j)=sen(6);
    
32.    pwm(j)=sen(7);
    
33.    
    
34.    low=j-200;
    
35.    if(low<1)
    
36.        low=1;
    
37.    end
    
38.    up=j;
    
39.    
    
40.    x_1=x(low:up);
    
41.    error_1=error(low:up);
    
42.    piterm_1=piterm(low:up);
    
43.    diterm_1=diterm(low:up);
    
44.    pwm_1=pwm(low:up);
    
45. 
    
46.    set(hErr,'XData',x_1,'YData',error_1)
    
47.    set(hPWM,'XData',x_1,'YData',pwm_1)
    
48.    if(up<200)
    
49.        up=200;
    
50.    end
    
51.    axis([low up -500 1010]);
    
52.    drawnow;
    
53.    j=j+1;
    
54. 
    
55. end;
    
56. 
    
57. figure(2)
    
58. plot(time,input);
    
59. hold all
    
60. plot(time,error);
    
61. plot(time,piterm);
    
62. plot(time,diterm);
    
63. plot(time,pwm);
    
64. legend('input','error','pIterm','dIterm','pwm');
    
65. 
    
66. fclose(s1);
    
67. delete(s1);
    
68. clear s1;
    
69. 
    
70. catch exception
    
71.     fclose(s1);                 % always, always want to close s1
    
72.     throw (exception);
    
73. end                  

复制代码

gantree

有动手能力的实践者都是牛人。。。赞

ysit1990

johnsonzzd 发表于 2012-5-21 23:45
完整版，用串口向PC机发送信息：const int SS495_PIN = A0;  // Analog input pin that the SS495 is attac ...

用matlab 来图形显示串口数据 这个满好的
楼主能不能    把这部分内容展开一下  

johnsonzzd

ysit1990 发表于 2012-7-27 13:23
用matlab 来图形显示串口数据 这个满好的
楼主能不能    把这部分内容展开一下

大部分是常规内容。关键的显示部分：
   设置数据： set(hErr,'XData',x_1,'YData',error_1)
   强制刷新： drawnow;

pww999

楼住什么时候放 Arduino的自平衡车和两旋翼模型 上来啊，好其待哦{:soso_e154:}

麽麽茶㊣

求详细教程~ 好想做啊~
动力老男孩博客里面怎么只要一端做线圈呢？

johnsonzzd

详细资料已经上传到百度文库里了，正在审核中。感兴趣的朋友可以搜索：
  杨麟 - 磁悬浮下位机软件毕业设计
  来俊鹏-磁悬浮系统下位机硬件设计

自平衡车和两旋翼模型已经完成，但还不很完美，需要修正。正在整理中。