PID 循跡自走車範例

1. #include <Arduino.h>
   + i4 t3 f7 \1 p" i( T( {% \
2. #include <Wire.h>
   0 n  v4 S5 P0 s7 s
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   4 X) g) t9 G) k( N
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   9 R, v5 M. h2 P' G
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ! u! b" G4 b' Q' ]& h9 W' x4 F
13. float Error,ErrorAcc,ErrorDec;
    + r, z) t/ M$ p3 Y, {, Q0 }
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    , S7 t' N* g9 V% c1 \- |
17. float Ki=0.000201;
    - `! p5 d8 f) t7 r, }( @8 |
18. 
    
19. float nPower;
    ' |* ~5 b/ R, q, _) N' Y
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    & t. P0 i- s. v( x4 D  J
22. MeDCMotor motor_9(9);
    ( a7 x* E7 B# Y% Y. _3 H: j: p- X0 F( W
23. MeDCMotor motor_10(10);
    . v8 P( u% ]) A3 S
24. unsigned long previousMillis = 0;
    - K' a( {$ k) O- `0 }6 Y
25. const long interval = 1;
    
26. 
    4 d3 e: G0 r3 u8 K/ z* p3 n3 b6 Y
27. void setup(){
    2 H+ h) Y; R& c$ n! _+ j3 O
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
      h8 @' J* \8 @* I  r) m3 _( Y# E
33. 
    
34. void loop(){
    ) T2 }$ h8 t! t$ k
35.   unsigned long StartTime = millis();
    : G, G5 \/ L* S% j; f
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ; @5 C, S2 D" J+ W, g
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    * N, r0 S7 p) _  X2 m
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    4 S2 E4 ~0 s' _& N
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    + [. w; f2 z/ F4 o( K" I
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    8 E# h9 f. g# P5 [7 X# x
43.         nPower += 3;
    ! V7 b7 E- |3 b  n$ B* I$ U1 l
44.       }
    - {) _. t8 h3 g; w4 q# |
45.     } else{
    
46.       if(nPower > MinPower){
    $ b" Q1 \. _3 C: `5 x( I# T
47.         nPower -= 2;
    
48.       }
    
49.     }
    - X+ I5 ~: d& K4 J
50.     MotoL(nPower-Error);
    ) _8 g+ \& S5 ?) T
51.     MotoR(nPower+Error);   
    
52.   }else{
      P, X% f: ^$ y- n, O& ^7 G- |
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
      s2 Y  c4 m) N  ]" O1 n; Y
57. }
    
58. 
    $ {. k& a1 p2 S7 P
59. void MotoL(int Power){
    1 ^7 |. _( r0 U
60.   if (Power > MaxSpeed){
    $ l* C- x: Y, n! @+ D' M; O
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    6 o+ F& ^' Y6 @1 c4 E: e% F
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    6 @! X8 v( e3 f# ]( \% w
71.      Power = MaxSpeed;
    " k: q4 U, ~: l4 T1 j* c  ?
72.   }
    
73.   if (Power < -MaxSpeed){
    ; a8 m6 W/ Q2 A1 i' [7 r* ~
74.      Power = -MaxSpeed;
    
75.   }
    8 s" X- m8 k; g  g) S3 S7 q
76.   motor_10.run(Power);
    
77. }  

複製代碼

2 ?. }/ w! L6 [, j& ?

( v5 Z4 i5 c. I8 w1 H" d您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
4 Q( S1 @$ Z" G% s& d2 B% d7 g3 S- l
但基於想追求更精簡的程式所以還是想請問一下
0 Y) o' i. a( y$ a* j/ I" }就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
, s4 B0 Q; _( n不知道您是否願意教我可以如何處理

$ D9 ^5 x& z% X- K8 A, Z
4 a: ?1 n0 I* Q以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    8 {4 n# ~$ g& T
12.   valuesMM = analogRead(MM);
    $ i2 R3 z/ A% D* z- L. C
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    7 r, e' Y  I9 i% F" R$ X
15. 
    4 `# L# `6 u+ f
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    . N5 N( A' Q. @( L& x8 y
19.     valuesMR = CNY70Val;
    $ e6 B: p, m4 L+ h- g1 ~
20.   if (valuesMM > CNY70Val)
    0 s/ ]9 Z3 K1 y, a3 a# {
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    # `' e* _! O& _# g) N  s9 X0 W
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    3 {# d' Q$ d1 O$ t* h9 {
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    2 Q5 a0 y1 X5 G# K4 o0 g9 u/ L' k# `9 s
28. }
      G" X* i7 x- y( {/ I- C
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    ) e$ L, Y9 T; D% O
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    , v0 g5 I6 ?+ H% {
38.     olderror = error;
    ' |+ L. s- v- \& q, m# ]
39.     int power = error / 5 + interror / 10000 + lasterror;
    ' a  @0 F( _0 B0 q0 h" N
40. 
    
41.     if (power > MotoSpeed)
    5 E- X/ B7 \# |' a* M
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ( {4 x/ ]. p' ~3 I' q( u
44.       power = -MotoSpeed;
    3 g3 v" \5 f8 [" c/ Z/ B* l2 q
45. 
    : q8 I) K5 K* z: T
46.     if (power > 0)
    6 X( I( I. [% b! l, r
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    : r5 \2 A3 j6 I$ z* G
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    9 I* a) D  A2 w6 N' E1 V4 m+ l
51. }

複製代碼

感謝分享

感謝分享