PID 循跡自走車範例

1. #include <Arduino.h>
     Y; S; p( T6 t! U5 n( d9 N
2. #include <Wire.h>
   
3. #include <Servo.h>
     Y' c/ D- C5 c# B3 G  Q/ C
4. 
   " o3 ]% `' j; r2 E! a4 T
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   ; H( O7 h4 ^: L1 V' H4 r6 s
8. 
   
9. //double Input, Output ;
   ! o  I% }1 `3 {* w3 m, o
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ' l2 X* }: ~2 A$ h% o0 ^3 x5 e
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    ( C- x! Y% q5 e# e9 e7 S# A
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    & A/ [6 P3 S3 l/ S' ]
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    % {/ l/ f! X5 k" l, G2 ~7 R+ W1 O
25. const long interval = 1;
    
26. 
    
27. void setup(){
    " M- G' W& C- A1 P- v
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    & I6 e/ ]% ^, N. g& g; e) K
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    $ x, S* ^/ k7 [3 m* C( S& O
34. void loop(){
    . l8 ]1 c! R1 E! i& v
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    $ s2 I& _; o( f7 _) d
38.     ErrorAcc +=  nError*Kd ;
    ' T( _( r2 M' y* P$ E3 u
39.     ErrorDec -=  nError*Ki ;
    & _5 }0 B3 v& P0 \! ^# m
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    8 ?( B! q; I6 T; [4 q% h
43.         nPower += 3;
    
44.       }
    3 q* J: X# }! M/ P% n
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    / r+ J& {, D4 m8 }/ P( A; M9 D
49.     }
    
50.     MotoL(nPower-Error);
    ( |" \( w. G0 x. Q) ^. C
51.     MotoR(nPower+Error);   
    , P2 T. H% F/ j# E% d; s
52.   }else{
    
53.     motor_9.run(0);
    2 f- z. V7 \7 m; r
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    - ~4 A( b8 A" U& J* M( Y0 z1 I# ?
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    5 _! X' z: I  E+ f  j* Q  G( E/ S
61.      Power = MaxSpeed;
    
62.   }
    + ?  r* x- I+ Z* e9 z  x
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    8 W; g" T0 }9 g( @
66.   motor_9.run(Power);
    , G9 A& g4 U( @! \& j3 j9 Z) h! B) |3 n
67. }  
    
68. 
    
69. void MotoR(int Power){
    ; N0 e3 M! b! M4 @
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
      A4 |& n( m) ~8 l) e
72.   }
    
73.   if (Power < -MaxSpeed){
    ; I+ g* J6 e: z6 b/ w
74.      Power = -MaxSpeed;
    
75.   }
    & ~* t7 f. ], F' K) ]0 K- n1 g
76.   motor_10.run(Power);
    
77. }  

複製代碼

: ]/ X1 R, ~9 i" y) R

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
. L1 ?5 ~2 p3 I4 c/ ~
' ~7 q) l8 T% h* n但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
+ m+ f5 u# y7 m3 x: _不知道您是否願意教我可以如何處理

* }* K+ {+ X. y$ T  K+ O
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   6 f  Z* ]$ M; L9 |
3. int interror = 0;
   : {- [; ~5 |+ R9 R9 Q
4. int olderror = 0;
   ( u; l7 i6 n- m
5. double values;
   5 P+ Q5 }9 B0 Q
6. 
     n% z$ ~& X1 T( Z( m3 Z) M
7. 
   
8. void CNY70()
   
9. {
   ! R3 k6 R+ v7 a  a: k- |3 o. w
10.   valuesRR = analogRead(RR)
    9 [& f! ?# m' s4 N1 u$ k3 |
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    $ z% _6 \: c0 D2 m6 F' R( K' t
13.   valuesML = analogRead(ML);
    4 k: H8 |6 p: b; H2 d1 R! P
14.   valuesLL = analogRead(LL);
    
15. 
    , q8 e  C# S+ I  l" m9 `& b
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    ( \4 }. T5 k9 K% _
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ' S# }! k5 e9 y& U8 c+ P* K
20.   if (valuesMM > CNY70Val)
    & J, K: d7 i4 F7 e+ Q
21.     valuesMM = CNY70Val;
    6 |4 z8 L; B$ E. Q. N+ ^( ~
22.   if (valuesML > CNY70Val)
    & ?/ U: Y( s* [& B: A
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    , z9 o% [' p$ [4 k& p' ]
26. 
    & \& m4 f& Q& _$ f/ I2 Y4 {$ v
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    & J( b1 G7 o0 Q3 p4 {+ J
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    & Q; a9 r3 l2 u6 d! v# B* k8 H3 h
36.     interror += error;
    
37.     int lasterror = error - olderror;
    - G9 _- q- G6 B
38.     olderror = error;
    , }" g# Q0 ~4 c( C* P8 |! z
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    + b2 a* I& ~# h$ o. Y2 [
41.     if (power > MotoSpeed)
    $ G+ Q" c9 S' _5 y7 Y8 d* K% j
42.       power = MotoSpeed;
    4 p$ ^1 ?% \/ X. W+ k
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    - }  |* p$ l5 M
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    / ^/ V! ]- @7 |5 y- x1 B( X8 o8 f
50.   }
    
51. }

複製代碼

感謝分享

感謝分享