PID 循跡自走車範例

1. #include <Arduino.h>
   % H% y4 K. {* ~- b& w$ q
2. #include <Wire.h>
   6 l4 b/ L6 ]/ Q, a
3. #include <Servo.h>
   . @1 F0 F+ r4 u% M0 \1 \& @
4. 
   7 L8 g0 E  a$ L0 ?" g2 V
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   & U$ i6 V2 [8 O, h$ A% z$ H) V
8. 
   
9. //double Input, Output ;
   2 o1 J# n' x2 d+ z* h& [: [/ T3 X
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    7 H3 W8 }0 g. m
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ' G! X: G2 U9 P, ^' h; n8 t! N
14. 
    
15. float Kp=0.14;
    4 r% a: }7 m2 W8 {# K( K( \
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    " P4 D0 B5 C+ W0 e0 p* S$ i
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    . G3 W; ~# }) M( |/ z5 z2 P
23. MeDCMotor motor_10(10);
    & X  D1 F  L2 D: t3 t
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ' A0 O0 ?9 I- t
26. 
    ; F7 X& j+ E4 z, y1 ?. C6 C# J
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    ' y) s" S$ W, |5 U5 ?
32. }
    7 C/ K6 t$ F9 ]* a& u
33. 
    1 o1 q7 d) `0 L: d
34. void loop(){
    8 i$ U, a$ j* @2 P& F% \. {& D
35.   unsigned long StartTime = millis();
    5 P9 L6 o: n% h' R, C
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    : }& C, f4 b* r( \% h0 n
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    3 U$ @% v6 H* u' ?2 K$ N
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    1 J0 S( d. D+ K6 C9 i& [
48.       }
    
49.     }
    7 T5 K7 f3 x2 R$ G  r- {5 k
50.     MotoL(nPower-Error);
    ; G: F- S, P9 L) O" z
51.     MotoR(nPower+Error);   
    
52.   }else{
    2 ^% p. V  q1 Z' n. x
53.     motor_9.run(0);
    ! W" W" A. o3 g2 E! \* ~
54.     motor_10.run(0);
    9 B$ Y6 [# w9 [7 v6 c
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ) V) r0 a0 a; o- o, r& {
57. }
    
58. 
    
59. void MotoL(int Power){
      c6 ?, ], z+ q9 L8 J
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    $ A# _7 T$ G7 L# ]
64.      Power = -MaxSpeed;
    $ y6 z) H+ n; ?3 C" R- g
65.   }
    , d, U$ m5 z. ]  W( J3 n" ^
66.   motor_9.run(Power);
    
67. }  
    . O7 Y8 t" i4 c4 t; S
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    2 _1 z; V  ~. b* N9 F* D# p% N
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

( J0 a+ I* r7 [2 ^, ~' N* y/ b

您好，不知是否能向您請教。
9 g8 ?  @5 z: ]! _0 J目前和宋修賢老師在處理Ardui Car
6 K; p( D1 |2 T6 s; E雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 ^5 h( B9 l7 H6 m
7 y2 Y" G5 t( v* @但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
$ O3 J1 x; f3 I6 {不知道您是否願意教我可以如何處理

% s% n4 Y& @# v
9 |' w8 y" m6 h) z5 C# y以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   1 L( O7 O+ R1 X- @9 r9 q
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   ( k1 Y" A6 y) d& L- ]7 b
6. 
   / Z* P: d6 B1 t8 C; w! j
7. 
   
8. void CNY70()
   & m& Y& X3 P) n3 G9 D
9. {
   ' N, u! g% o* ]) ?0 H
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    - w! `) M- q8 X6 C
13.   valuesML = analogRead(ML);
    : `2 j. K, _$ Y8 c/ t0 t2 i' g  N3 B
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    - |5 {4 G5 H4 `" {; D
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    * c( q1 O+ k: N. W% Y" e7 f8 \- V
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    3 W$ Q) r( T/ F, L. }) f
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    " ~0 A4 V: f" F2 ~7 l% X
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    6 B9 Y% n# |7 {5 c' Z  @0 L
28. }
    7 O3 ]- x) {3 ^+ ?3 E
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    8 R/ p1 v, v) E4 {
33.     CNY70();
    
34. 
    3 J- ^: D. c+ R1 V2 ~4 @; G8 i
35.     int error = ((int)values);
    
36.     interror += error;
    ) \( j, d! W' k7 K
37.     int lasterror = error - olderror;
    * e( g" t$ N; E! S8 y! r: X0 u
38.     olderror = error;
    ! n. ]7 _1 U/ G# B# r. _  L/ Q% \0 x
39.     int power = error / 5 + interror / 10000 + lasterror;
    5 [3 U4 g  K: s, N5 o: ^
40. 
    : F8 Q3 T( b- R) L  f( M
41.     if (power > MotoSpeed)
    + |; L5 K" y/ ~4 o: |6 }  {' B3 ^7 `
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    / j4 v" z, Z! x
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    2 M3 q# }! ?" q+ `
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    , K/ q. D) ~; k# x% W! b( H
51. }

複製代碼

2 D* L% R+ L$ q, f

感謝分享

感謝分享