磁悬浮Arduino代码

1、折腾了几天终于把这个东西搞定了。看似简单，还是有一些小技巧的。主要材料：1. Arduino UNO主控2. SS495线性霍尔采集位置信息3. L298N驱动4. 直径12.7mm强磁球5. 电磁铁12V，线圈电阻10欧主要参数：PID控制，采样周期1ms，PWM频率 3921Hz，悬浮距离30mm，电流300mA筷子版的，下面是一个磁体，上面是一个小铁块，中间是一截筷子。悬浮距离比较小，但是方便进行建模和控制理论分析。 磁球版，悬浮距离很大，不容易进行理论分析。 ARDUINO 代码复制打印1. const int SS495_PIN = A0;  

2、/ Analog input pin that the SS495 is attached to2. 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.

3、   / 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

4、(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( )

5、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.   t

6、imeChange = (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

7、=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.  

8、;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.

9、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.   else72.     last=r;73.  

10、0;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的自平衡车和两旋翼模型，有空也放上来。完整版，用串口向PC机发送信息：ARDUINO 代码复制打印1. const int SS495_PIN = A0; 

11、; / Analog input pin that the SS495 is attached to2. const int PWM_PIN=3;  / Pins 3 and 11 are connected to Timer 2.3.  4. const double SampleTime=0.5;  /ms5. const int PWM_BIAS=128;6.  7. int Setpoint=850;8. double Kp=1, Ki=0.01, Kd=90;9. double piterm,diterm, iit

12、erm=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 t

13、he 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 setupC

14、ontrolTimer()26.   27.   /set timer1 in CTC mode.28.   /"ATmega4_88_168_328_DataSheet" , section 16.9.2 Clear Timer on Compare Match (CTC) Mode29.   cli();          / disable global interrupts30.   TCCR1A = 0;&

15、#160;    / set entire TCCR1A register to 031.   TCCR1B = 0;     / same for TCCR1B32.  33.   / turn on CTC mode:34.   TCCR1B |= (1 << WGM12);35.   / Set CS12 bits for 256 prescaler:36.   TCCR1B |= (1 <&

16、lt; 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

17、 global interrupts42. 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

18、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);6

19、8.  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(out

20、put,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.   

21、;r=analogRead(SS495_PIN);88.   if(abs(r-last)<4)89.     r=last;90.   else91.     last=r;92.   return r;93. 94.  95. void setup() 96.   setupCoilPWM();97.   setupControlTimer();98.   / initialize serial communicat

22、ions 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.   /      pro

23、cessCommand( Serial.read() ); 108.   /    109.      / print the results to the serial monitor:110.   char str50;111.   sprintf(str,"%ld, %03d, %04d, %+04d, %+04d, %+04d, %04d",now/1000, input,error,piterm,diterm,(int)iiterm,output

24、);112.   Serial.println(str);   113.  114.   delay(20);115. PC机监控程序。Matlab，能够自动滚屏ARDUINO 代码复制打印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. &

25、#160;8. try                             % use try catch to ensure fclose9. figure(1)10. hErr=plot(x,error);11. %title('Dados de LDR Aquisitados');12. hold all13. hP=plot(x,piterm);14. hD=plot(

26、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)<722.         continue23.     end24.  25.     x(j)=j26.    time

27、(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.    end38.    up=j;39.  40.    x_1=x(low:up);41.    error_1=error(low:up);42.    piterm_1=piterm(low