PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   ) p1 A; J' d$ }: o- E# u7 a! W( N
5. #include "MePort.h"
   * E" l) {' N  t' O! S* ]
6. #include "MeUltrasonic.h"
   $ b6 x7 ?, _$ R& b" }% z' T2 }
7. #include "MeDCMotor.h"
   / ]' A% f9 ~+ {1 P6 M2 O9 f4 \
8. 
   
9. //double Input, Output ;
   # \  }- y) G3 p7 Q
10. float MaxSpeed = 255;
    1 n8 O7 G# w0 C3 L% o8 A! i$ f
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    2 z) {2 {/ o" a+ e, h6 v
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    % N1 H$ M! |- s
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    7 `& H% K0 z  R" z# m6 ^; j
24. unsigned long previousMillis = 0;
    4 @( C3 x; x+ o- i+ e, g& s, m
25. const long interval = 1;
    $ H3 f+ ~2 [- O
26. 
    4 b9 D  v" u2 e# e
27. void setup(){
    ( O( F* `$ k% B- C  b0 K! _: Z
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    % [. y+ e1 I, h
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    2 R5 _8 j& V) v, `
33. 
    2 g" Z, R6 z( }7 e' p8 C1 I
34. void loop(){
    # j; e& L9 `4 N7 W- U
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    3 ?9 e& C- O" A6 B3 h3 V
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 o+ O, q" l" C3 ^
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    0 o8 y( [4 u- f7 @
44.       }
    8 V" D; [1 p2 `% i* s
45.     } else{
    
46.       if(nPower > MinPower){
    1 R8 e$ t$ X  x3 [! A7 @( f
47.         nPower -= 2;
    : I9 \; R* u7 Y5 |( N3 y# b
48.       }
    
49.     }
    ' F/ B4 V& {* M- }
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    2 L; f7 [2 t* _5 T6 D
52.   }else{
    . M2 f: Q# n; U) S: `- V2 g
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    9 K2 @3 j# t: V; r$ W7 ?6 l
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ( D, ~5 P1 `* b0 @* o
57. }
    ( m) u9 k8 A: `( [( E
58. 
    % w$ D' h* ^* ~; Q( f; ?  b5 Z
59. void MotoL(int Power){
    / {. t2 b. ^1 `
60.   if (Power > MaxSpeed){
    1 Y0 ~2 w, t1 X( d
61.      Power = MaxSpeed;
    
62.   }
    ' A  U& Y& ]+ z$ B0 Q6 L  y# c
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    7 R* g* m% V' r4 g! W% Z/ `8 [
66.   motor_9.run(Power);
    
67. }  
    - Y7 h0 r5 }4 P9 r! w
68. 
    - j* ~# l" n1 |* i
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    , p" v2 ^  a. @6 I7 |* I+ d* N9 D
71.      Power = MaxSpeed;
    
72.   }
    / U  q6 ]+ J) p$ L9 T; R
73.   if (Power < -MaxSpeed){
    * N2 }3 g, N5 |4 I' q, d
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    $ w% V6 P1 v- ?" I9 M
77. }  

複製代碼

# F* X5 e2 z6 A" W- I4 _3 R( F您好，不知是否能向您請教。
' }1 E. ?- `9 |9 M) _目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

& R- Z. ?( `6 N( ~! S) Q# V0 n! a但基於想追求更精簡的程式所以還是想請問一下
' d: q% X$ U& _! d& e( u就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
5 ?+ _8 |( [' ^6 U% w7 `4 y

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

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   / Q$ M# Q* w+ E& L! `3 n, u) v
4. int olderror = 0;
   
5. double values;
   
6. 
   5 I, h, S8 Z0 c1 h1 e# A
7. 
   
8. void CNY70()
   
9. {
   , k' K/ u# {- V1 w+ F" f$ t
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    / z" }! S' d$ `# M
12.   valuesMM = analogRead(MM);
    % \6 U; W7 X+ r) U& _4 M
13.   valuesML = analogRead(ML);
    - x9 K' O, K7 `6 B4 G; w/ m
14.   valuesLL = analogRead(LL);
    9 m# F- l6 |" t. R3 v
15. 
    
16.   if (valuesRR > CNY70Val)
    1 h9 O- W, D( p3 H7 ?3 D$ W
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    % M8 E! c$ s1 m8 t( ?5 @; w
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    & A" ]+ s& U" M5 y+ e
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    # `; t* j# U; l, Y
25.     valuesLL = CNY70Val;
    4 G5 ^  h- h9 `* S- M. S
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    & z' t; B. D; _9 `, f  f7 u: @' K
30. void Car()
    
31. {
    ; v4 ], ~) I0 p- D% _8 @8 H% D
32.   while (1) {
    
33.     CNY70();
    8 G' ?' N( b4 a9 `$ M
34. 
    . k% Z( n! j( v, f
35.     int error = ((int)values);
    " H: L. s# k. `5 ~8 m' m
36.     interror += error;
    / j; E; p! v) ?8 B& d  b# A
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    / W( c+ U4 }' b! c5 W4 @- T# c
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ; M! v3 h: \* T' u8 \  c/ K) S
41.     if (power > MotoSpeed)
    7 U) r4 o3 F' O$ c/ m
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    3 o; H* c% t5 ?; V
48.     else
    ) t- R# ]0 D4 \! l0 f
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    8 Z! z, t( {% l# [7 i
51. }

複製代碼

  i; V; |) s- @# C2 d

感謝分享

感謝分享