PID 循跡自走車範例

1. #include <Arduino.h>
   , m- Y0 K6 \+ x7 L- d7 h
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   5 F. r# Q& ?4 s3 L5 s* N: |# X
5. #include "MePort.h"
   % ?* n4 R) d$ a0 F/ F& O2 T( J
6. #include "MeUltrasonic.h"
   8 \  T" [, m! t4 \/ x) F8 D, M  x
7. #include "MeDCMotor.h"
   " Q5 R; J9 ~! a: Z# }
8. 
   
9. //double Input, Output ;
   3 a% y: I7 A, p$ |) C
10. float MaxSpeed = 255;
    3 c8 ~' I( V4 r8 ^0 w9 b2 z
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    & n! N! c/ }+ N0 n7 K4 E9 ~
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    ' c; Z, p; _3 R8 {. U
17. float Ki=0.000201;
    ) ?7 V$ _8 g' ?9 z; x
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ; C( e# ~4 Z0 y4 J
26. 
    
27. void setup(){
    ) z- _+ ?$ }, }7 K
28.     lightsensor_6.dWrite1(1);
    ) h/ j. F) T9 K
29.     nPower = 160;
    
30.     Error=0;
    * X* w. J' O8 }! z3 j
31.     ErrorAcc=0;
    * f  S4 I0 t- B) S' ~
32. }
    % X. l' d$ w- |5 m* ^
33. 
    
34. void loop(){
    # S: n* a0 ^$ `8 B7 p" w4 @
35.   unsigned long StartTime = millis();
    / z5 L) s/ }$ W+ P# J
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    " F/ V" g) W( {  V
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    ! m% R7 p; [- F" C. r# I
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    ' \, p- F9 t  p+ b- }% q' ]8 S) T
42.       if(nPower < MaxPower){
    1 Y5 d# G6 _& D1 [* B/ L
43.         nPower += 3;
    
44.       }
    ' t" ^% }- m. c. [
45.     } else{
    
46.       if(nPower > MinPower){
    / _0 _  w8 }# V1 j
47.         nPower -= 2;
    
48.       }
    9 J% a' q. f, f+ y: [8 T+ `1 p
49.     }
    . Q- `; Q$ T% b: s; g9 ~
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    / W. b1 `& U/ y( D/ k! C& `& J
54.     motor_10.run(0);
    
55.   }
    3 {( P) ~& W" R/ R
56.   do{}while(millis() - StartTime < interval);
    ' W, `- S  m5 K) F
57. }
    
58. 
    
59. void MotoL(int Power){
    $ m8 i2 V) a* n: |3 i
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    1 F5 e) o/ x4 H2 E1 X
62.   }
    
63.   if (Power < -MaxSpeed){
    3 A8 V) U- g8 H: [
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    # [8 Y8 R9 g" z
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    9 E" y+ t  h  D( J" ^
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    4 \5 }/ \2 r; I4 C: n
76.   motor_10.run(Power);
      [' `6 l1 m' B! @% G+ H7 \
77. }  

複製代碼

. m. p" [0 b/ }0 C9 C
4 U9 @+ h1 j3 M) N" a: l您好，不知是否能向您請教。
+ k6 r' `, [& p6 z  l: K  l2 t目前和宋修賢老師在處理Ardui Car
& D+ }& ?! ~/ \, c" @0 ~6 ]雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 j" j! G& q5 k. n. G/ `
' ^9 c! A$ ]9 ]* c: l" |但基於想追求更精簡的程式所以還是想請問一下
' n% q. t3 m- D+ v就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
5 e$ f5 p/ w) C3 |. C: q+ ?: G不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   + x+ n% w  G' U- x/ f8 Z; F6 z
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   2 Q5 q% ?! C, B) C2 a- ?
8. void CNY70()
   * S- y/ e3 a# _  k( H/ |# p
9. {
   6 u5 f( W: Z; R5 K  \' G
10.   valuesRR = analogRead(RR)
      Y) |5 \* H- S3 K+ f
11.   valuesMR = analogRead(MR);
    1 |/ K! B7 U7 K( O+ t& I
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    5 Z2 }+ b* Q( {5 V3 D" }: |4 ^% D
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    * @  a4 C3 z3 E* S1 M
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    - f8 w7 x, d5 C  s2 K
19.     valuesMR = CNY70Val;
    7 q% `; M( T5 X
20.   if (valuesMM > CNY70Val)
    3 b; z7 k# X: x1 K- q: u6 Y! N
21.     valuesMM = CNY70Val;
    9 q+ s9 F" Q# g+ M$ x9 S; m
22.   if (valuesML > CNY70Val)
    ; }/ t7 U7 b! C' m8 ^+ d
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    % N5 m4 y- _3 E6 D# z- I
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    % k- N" b$ Y, A
28. }
    ) z% x  C1 a) _  r
29. 
    
30. void Car()
    
31. {
    ! f; c, l9 _) k* B: k
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    2 r  ~1 I  d9 K
36.     interror += error;
    
37.     int lasterror = error - olderror;
    ' H# Y- f' j# L- [- u: o
38.     olderror = error;
    ; c) _1 @  G$ f% k
39.     int power = error / 5 + interror / 10000 + lasterror;
    ! G5 a  p, x: W* d- S
40. 
    5 R+ ~5 d3 E5 I% A" q7 E" D
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    9 W$ \4 d4 m! d/ x: T
46.     if (power > 0)
    : ]- G4 a" d. E( |# ~/ ^
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    + ?, l- l6 s, ?; V
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    % e8 ?! M* }3 P) ?9 @- ~8 o, Z1 ^0 P
50.   }
    
51. }

複製代碼

感謝分享

感謝分享