PID 循跡自走車範例

1. #include <Arduino.h>
   ' e2 m- ^* p3 Q1 ?
2. #include <Wire.h>
   1 q6 |  x! S- i1 p6 M# t' N+ I9 p; {
3. #include <Servo.h>
   9 f4 _8 r1 H# V4 Y! b- G5 p
4. 
   ! Z  w3 A! V# y2 i+ [
5. #include "MePort.h"
   % A% W& l/ h6 L5 ]0 w
6. #include "MeUltrasonic.h"
   5 ]1 o8 e, k, A5 H$ C" P% }
7. #include "MeDCMotor.h"
   " J6 i5 a" i& g
8. 
   
9. //double Input, Output ;
   ( ?  D% H1 m" c: V, Y; M+ ^
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    7 \" }9 C. f9 S, g! B
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    2 t# K+ Z& ?* f% s3 ~
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    1 @5 X8 J+ X* Y& N
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ' _2 ^, @- M! V
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    # Q! k1 J. k! B
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    & x5 b8 H9 z4 ~8 ~' `
29.     nPower = 160;
    
30.     Error=0;
    ! B; M' p% n  a6 i( H
31.     ErrorAcc=0;
    
32. }
    " c- s% l. Q0 k
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    ' f$ T$ Z. T) V# h. s1 @5 g
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ) [% C3 Z0 D, q2 o4 ?. L
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    # q) F7 J0 {: |$ M
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    - b# ^9 W! a+ s4 B
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    * X& N2 F' h4 N6 a6 O
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    # O2 G# I7 v  T; [
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    5 M3 b/ L( q2 h  Q, h
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    % X* }) e/ x( v5 `$ n- t
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    9 n: L' w$ w8 M4 r) \
66.   motor_9.run(Power);
    9 K& L4 `% K2 u7 \, A4 F
67. }  
    
68. 
    . [' A" E# [1 c0 o. h5 s1 {( u
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    0 ?5 A; t5 W' e
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    - ~6 R0 }$ d$ P$ R! c1 L
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

您好，不知是否能向您請教。
% ~. P. Q& N+ y% U" P8 o/ p目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

: r, |% s! O/ z; J- D. X但基於想追求更精簡的程式所以還是想請問一下
+ w7 _3 H& T/ ]: o, m' G就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
# i4 g' }9 P2 z: W不知道您是否願意教我可以如何處理

$ O, O0 A; v1 `9 R* F7 x6 b4 h* [
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   0 D  Z5 {1 q1 n3 L
4. int olderror = 0;
   ; c0 b' Q4 w+ f- T1 R5 T9 A. u
5. double values;
   
6. 
   0 {" A% i/ |& f1 B1 Y
7. 
   ) Z$ `0 I$ l' y' ~6 \6 _
8. void CNY70()
   ' y% e( q, _1 @
9. {
   5 N9 P! c5 s* H9 `- B1 n
10.   valuesRR = analogRead(RR)
    * N, ?# g6 h: D! M/ `. h
11.   valuesMR = analogRead(MR);
    ; L4 w9 p# t7 L- C5 j, `
12.   valuesMM = analogRead(MM);
    " y  u# h" ?5 K7 z) N
13.   valuesML = analogRead(ML);
    % j  G. Y, T  [' ^. ]$ f$ @: j
14.   valuesLL = analogRead(LL);
    0 C) p7 N* D4 a3 o; Y. E
15. 
    2 C. X; j- K5 u3 ]3 x. G
16.   if (valuesRR > CNY70Val)
    ( Z, \. W$ M7 o8 C$ q
17.     valuesRR = CNY70Val;
    7 N+ E0 A: |0 S2 s, p
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    5 y1 B& O$ e8 N+ Y
20.   if (valuesMM > CNY70Val)
    0 x( P5 e% N( h9 k7 E* M2 R
21.     valuesMM = CNY70Val;
    - E- o' |( u4 l
22.   if (valuesML > CNY70Val)
    1 I5 U- T, l) L" [7 D' @. f) p! W- o
23.     valuesML = CNY70Val;
    7 W) i: `1 ~; x# ?
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    3 ^) A5 }2 m/ l$ o$ d  H
26. 
    / c5 X% \2 H6 \# n6 o* R6 W
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    0 g8 Q  r- Z7 n& A" v" \' A
28. }
    
29. 
    
30. void Car()
    % q1 V, }, n. k1 |2 g+ C) {9 r
31. {
    4 x0 I# T' ~- W" S9 p5 d
32.   while (1) {
    
33.     CNY70();
    0 q4 W9 Z! U+ H( a+ F" U8 ]
34. 
    " g9 k. R5 }9 e
35.     int error = ((int)values);
    3 F. F9 b9 ^% q' l" o" w1 E
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    / q/ A2 l! Z% e8 h# {
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    1 I( w$ N3 b) e  ^" p
41.     if (power > MotoSpeed)
    + }- m( B% R+ [7 w, |/ l; b
42.       power = MotoSpeed;
    * O5 A: N4 n9 P8 @  l
43.     if (power < -MotoSpeed)
    ( D& e* v7 ?- _1 H4 p
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    / x6 I2 j* E/ y7 `1 m6 E; b
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    & w; K; R& M& l; m2 j; j
51. }

複製代碼

- T# v& \  n( \$ w- r2 q

感謝分享

感謝分享