PID 循跡自走車範例

1. #include <Arduino.h>
   : W2 ^$ Y* x- s  F) h3 [! P8 ~
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   % p, `* X& D+ |  e: I) v
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   + y- L' D0 `2 z7 z: z. B" T4 X, q
8. 
   
9. //double Input, Output ;
   $ @& m6 t  T8 q( V& _
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    9 v, R) [$ b, L& H
13. float Error,ErrorAcc,ErrorDec;
    ) J/ w$ G/ B4 e2 W9 q. i! p* I6 U
14. 
    
15. float Kp=0.14;
    ; ^0 P' J( b9 V; q/ \
16. float Kd=0.00020;//23;
    - P, B" x2 R, |4 g/ n
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    3 N/ p  }2 p; g( A/ ?
22. MeDCMotor motor_9(9);
    # C- F  X8 L- ]& g
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    1 d$ z" S/ G, U9 _3 q$ m: L% j
25. const long interval = 1;
    9 c0 s- y  y# X" g1 J& x9 s/ W
26. 
    
27. void setup(){
    1 A& W2 g& q" z2 E1 B
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    6 C  x4 j1 E: R
30.     Error=0;
    ) b' o# B) q8 x6 B
31.     ErrorAcc=0;
    " \1 i6 O& |8 T" }
32. }
    
33. 
    # z) N% ]; ]. @5 K- j
34. void loop(){
    0 ]: O! M+ c: z8 p& @) N
35.   unsigned long StartTime = millis();
    $ t. b, A! U0 M8 O5 j, D/ A5 J
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    7 q0 M5 c* s) x
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    ) ]. M9 i# f/ I* ~0 Z4 a; T: Y
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    + {5 l' l. \5 ~* v% {( p
42.       if(nPower < MaxPower){
    & v" X3 ?  ]7 T( F) O0 X# @( L
43.         nPower += 3;
    0 P( U: e, c- X6 B: Q
44.       }
    ( Y' T/ L  }& b8 _# Y; \% U
45.     } else{
    
46.       if(nPower > MinPower){
    ! V( q+ Y+ p2 J
47.         nPower -= 2;
    ! X1 _* t5 g( f
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    7 b6 ^4 N/ Q$ j6 V
51.     MotoR(nPower+Error);   
    9 N. h6 V0 l- `  S0 V* K) [, p1 k6 N9 ?
52.   }else{
    
53.     motor_9.run(0);
    7 i+ y) p8 L, X1 J8 s
54.     motor_10.run(0);
    6 N9 |- W4 D" x8 x8 M( v# t% l
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    5 |3 J7 B9 O0 r, r' {$ n) e
57. }
    
58. 
    + F3 X! D5 S0 h- U8 W: P
59. void MotoL(int Power){
    3 h/ d/ j! M& }1 D) R" u2 I' I
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    3 u1 B: F# i1 X! B( l
63.   if (Power < -MaxSpeed){
    & ^) a: U, O" }0 q/ E2 ~
64.      Power = -MaxSpeed;
    9 ]6 ]3 z, F! u( Q% }
65.   }
    
66.   motor_9.run(Power);
    7 E4 K) q% {9 }% V; Y! j2 z
67. }  
    " Y  H9 d: j+ @& Z2 }% }
68. 
    % H! M" T" ], ?
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    ) E; R" U( C. v' O+ g* S
71.      Power = MaxSpeed;
    ( B1 g7 ~0 B- F9 D
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    2 W" F, [+ K/ D. ~+ M
76.   motor_10.run(Power);
    ! p# o" z9 @+ F" ]/ f
77. }  

複製代碼

- \0 v" Q# I5 n( P( g( `

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
4 \9 o6 A( ]0 g8 r' l3 H雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
7 z& F! m' E, t# b/ o6 H# |就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

% k" a2 V" V4 |; o
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   5 a! x- S3 @! g7 e7 |9 U
5. double values;
   
6. 
   
7. 
   5 E; b: h2 g- o% ~6 ?, H3 G* e9 O- w
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    0 N$ c; O+ }  x7 h- p
11.   valuesMR = analogRead(MR);
    " M1 l; C' B0 W2 g7 ?
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    # p' g/ ^9 q: b
14.   valuesLL = analogRead(LL);
    2 u" C$ `- o' r0 z/ X4 k& f0 R
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    0 D" Y% T' w, W. c" T, K3 w
18.   if (valuesMR > CNY70Val)
    4 M: L* i, v2 c' ]" W+ F+ F
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    2 a, C/ f& Q0 A
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    $ w6 K* [! p; Y; m
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    - `' N6 Y  y$ l6 O; k" f
26. 
    8 Y8 j; X1 H! P8 ^# \
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    ' d. J- \5 z  {; Y; [, _& `
29. 
    
30. void Car()
    - v8 @* T  q8 m4 k4 n9 f
31. {
    - y) O( L" W8 S& P4 r
32.   while (1) {
    9 w9 i9 o6 U8 Y
33.     CNY70();
    
34. 
    5 D1 g( _% a) B# h
35.     int error = ((int)values);
    ( v; B' [( s3 o( d+ K
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    7 i6 S" n2 O$ {$ j( G
39.     int power = error / 5 + interror / 10000 + lasterror;
    / \2 g: O- D. ~
40. 
    + L1 e0 _1 |4 x  v9 \, H( j3 W3 m
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    1 U/ j# e1 M0 L/ U9 v1 d
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    1 _1 P5 n- \, O6 g, g1 \9 C
45. 
    
46.     if (power > 0)
    5 |- J5 ~( [) ^
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ! Z) {! _" s( P
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    7 `6 G( m  I4 |; q! w, q3 w+ i$ B$ ~
51. }

複製代碼

感謝分享

感謝分享