PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   8 y5 B& p! c, _5 K
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   6 T  M& t7 H8 c5 Z
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    # \% x; ?* @9 p+ ]
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    % z1 S: }5 v5 k. W
17. float Ki=0.000201;
    
18. 
    7 _1 W* n. d* q! m# y
19. float nPower;
    9 s2 L, R' l7 j. ~, ?) D
20. MePort lightsensor_6(6);
    9 A' H0 s/ Q  O) O& F0 i+ p0 z7 ]
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ; O0 V) F4 |: V  D
23. MeDCMotor motor_10(10);
    ' S9 E0 f( ?2 o4 T! p
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    8 X% {- N  z6 Q' F- _, F
29.     nPower = 160;
      A3 S8 s+ `( }- H7 m8 A# W1 ^
30.     Error=0;
    4 ^5 R/ J2 O' [9 Z5 N2 q. p% P" J+ g
31.     ErrorAcc=0;
    
32. }
    
33. 
    9 q8 M' n  z0 y' L
34. void loop(){
    
35.   unsigned long StartTime = millis();
      Z" X' m" a( Y% y4 i9 A( X
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & D) {& y$ R) T. }6 T* H
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    + u' M: g/ d1 F+ |0 D) y
42.       if(nPower < MaxPower){
    ' P5 `/ p' |) x: i- N2 p
43.         nPower += 3;
    
44.       }
    
45.     } else{
    # j2 L& ?& I" t  f
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    " H, Q3 e  `: Q( ?' l' w& X
48.       }
    
49.     }
    2 n5 v0 @8 T! l" b# H% j3 K0 ^! [
50.     MotoL(nPower-Error);
    ( V$ m1 i6 ?5 W& O
51.     MotoR(nPower+Error);   
    * X; R. ^- {' j* N  k5 v& e
52.   }else{
    3 g. c: T' L3 e
53.     motor_9.run(0);
    / }) c) h( m& ^5 t3 C6 }) C1 u
54.     motor_10.run(0);
    
55.   }
    7 f5 S0 V$ g$ m: X( h
56.   do{}while(millis() - StartTime < interval);
    ( K# W( {; f9 l3 i
57. }
    
58. 
    
59. void MotoL(int Power){
    / @0 V; p! P- C+ X) W$ D+ C
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    ; Q. q; `/ P9 R
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    * V# }' Q2 o' _( ^$ Z* V
66.   motor_9.run(Power);
    - J2 v: D1 P: ~
67. }  
    5 L  J: x' M1 z  u0 `" g/ H
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    ; B0 R" d& _/ r; s$ n; \
72.   }
    ' m' g: {. ]$ e3 r0 r" E
73.   if (Power < -MaxSpeed){
    $ O8 G" G" a; {* w5 Q+ g! j
74.      Power = -MaxSpeed;
    
75.   }
    - R( |- q; }2 r2 P% j) @- Y+ f, A% z
76.   motor_10.run(Power);
    
77. }  

複製代碼

' [' X+ A) n, S

; w% r& \! O# N/ e$ h
您好，不知是否能向您請教。
% t! Y0 w* w- m! W9 V: Y; _( V/ h0 B目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 X9 k( N5 \# n$ f" P* j; C. e
& v6 F; n  o! c, |, a3 t6 l但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
$ m! h- O) A5 V: z不知道您是否願意教我可以如何處理
! ]8 z. P8 M6 L3 x6 ~( n

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   8 U5 \, j, Q+ ^% K5 u
5. double values;
   3 H1 |' i# y& [; Y; _; ~& f. T
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    * l1 `) u, O+ O, |2 |5 g  N
11.   valuesMR = analogRead(MR);
    ' S; M) U, n2 ~0 d
12.   valuesMM = analogRead(MM);
    $ `2 I, t! ?$ I* s5 u% V3 p! e
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    2 ~; R- M5 @% G+ D) N2 s! w
15. 
    - [1 }" a/ l0 D  f) f
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    ( }7 E  D5 i7 K0 h
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    # w4 _. c% I- v4 z( k4 G* w( a
21.     valuesMM = CNY70Val;
    ( F( j+ @8 |4 p6 ~
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    , i/ }" X5 w2 M( O
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    ! k) |7 U; {7 D: S. N8 v0 o2 @
36.     interror += error;
    2 x3 G# P2 k2 V- J' j5 _2 _
37.     int lasterror = error - olderror;
    # V0 `1 c( e+ K0 f' z8 J5 U' u
38.     olderror = error;
    8 t! E# w! w) j4 k7 e( ?
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    * e4 V0 ~  V0 V  O
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    + ^. Z- N8 V/ ~& W1 ^& z7 f
45. 
    
46.     if (power > 0)
    5 Z# I. V0 J# d+ a3 [
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    + n# p/ t) T! |8 J  a' |& n; P
50.   }
    + K# P3 K4 Q" d' S0 X
51. }

複製代碼

6 u$ l/ P3 U% O7 F
7 [# x( F: g* z% Y* P

感謝分享

感謝分享