PID 循跡自走車範例

1. #include <Arduino.h>
   6 a* i7 t$ Z2 @" S7 O
2. #include <Wire.h>
   4 H5 v" Y+ }8 P* x3 J
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   2 I7 Z9 X1 N5 Y) r; o: v; O
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   6 K3 G4 W" }2 D& a/ }. O3 M
8. 
   8 P+ A1 e2 \8 h4 \4 c
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ; o: v1 t+ j4 f7 }. A' |) X: E' B" M% }
12. float MinPower = 120;
    6 n5 y1 Q* W) P3 a
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    & N$ i" m" \( G+ @# R2 T% o
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    $ g1 f2 j4 x/ E
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    : p# X* w; q- A9 T
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    / a  {( U) T2 d. `
30.     Error=0;
    3 `6 i) ?# N2 ~4 e- r
31.     ErrorAcc=0;
    % c0 s  `9 F. U& o  g1 t
32. }
    8 T" n% W2 b5 v" L# j5 u
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    / ^. G( y; O+ u
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . ?. D  @6 G! Z1 Z- r- Q/ c1 n/ H
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    - T% R4 h: V: x, W: l& }. X* a
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ i: ?. T6 q" p0 H! L" @7 U) x. B
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    " l$ ?( X1 g8 x6 `
43.         nPower += 3;
    
44.       }
    
45.     } else{
    6 u6 U2 n  `4 r# ?/ b' H5 R+ \
46.       if(nPower > MinPower){
    ' Q1 Y! j7 k) U/ Z( p2 ^* t8 o
47.         nPower -= 2;
    5 o1 ^/ v- ^5 O3 `  n1 c5 h
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    - I' J0 l2 P9 ^+ n8 i  w+ r
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    & o# G+ m, L$ p3 g% Y% i3 G. |0 L: Y
54.     motor_10.run(0);
    . x% f+ ~1 C9 m6 ~- J, d  d
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    1 q1 }' W8 l8 [4 i: S4 t( N* g! _* X! M  z
58. 
    
59. void MotoL(int Power){
    * U/ E, d8 M2 g2 p2 X
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    " `5 q7 O% `+ l3 U  E% J( h
62.   }
    
63.   if (Power < -MaxSpeed){
    2 A: N; j3 {9 s
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    , L' J& w% A% L  ]+ w0 B
67. }  
    
68. 
    
69. void MotoR(int Power){
    $ [! Y# V$ y3 I; W! t" y( Z
70.   if (Power > MaxSpeed){
    , \- [- R7 A+ b7 r; g8 P. z/ H# i5 d! u
71.      Power = MaxSpeed;
    7 O8 M: i8 ]/ J5 r3 k" P
72.   }
    6 k! s1 r6 X; |& d
73.   if (Power < -MaxSpeed){
    " N3 {  D1 b6 b1 }7 B' d( e
74.      Power = -MaxSpeed;
    " F3 j( D+ [* c6 J( ~( [& G6 S% z  B
75.   }
    
76.   motor_10.run(Power);
    2 o0 g& x. T3 p: o
77. }  

複製代碼

6 z3 R# x+ D. k/ I+ e

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
/ S0 G3 @9 T! @: t0 a4 b. I2 t雖然已使用較繁雜的方式處理了跑出黑線外的狀況
; k8 J0 B/ B# M+ b
但基於想追求更精簡的程式所以還是想請問一下
  K, Q# v. X; U6 e就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   ; m) d, ~2 n- ?5 c5 W9 O0 x7 `
2. double CNY70Val = 1000;
   5 T8 p  Q. _0 u4 K8 }$ t
3. int interror = 0;
   6 M) L1 v! |7 R: |9 b7 d
4. int olderror = 0;
   
5. double values;
   1 i1 d  Y0 i5 C2 v' ^
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    : y7 Z1 \; ?* y% Z  U, M
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    - {/ K; c. s( C; j! J
15. 
    4 p' o# a. S$ @% T; u
16.   if (valuesRR > CNY70Val)
    ; S* g8 k" v& U, C5 C
17.     valuesRR = CNY70Val;
    7 l, Q1 f. B9 }0 P; }
18.   if (valuesMR > CNY70Val)
    ' ?$ O+ m/ t( a) i
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    - Q( f, ?. [: {5 Q
22.   if (valuesML > CNY70Val)
    8 A* @, M0 _& k: T5 W
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    8 j) o! |; F6 k. z
28. }
    ! h- A, P/ L* V/ X. L3 h* F
29. 
    
30. void Car()
    
31. {
    % ^; M  T1 v4 }8 z% h; Q2 o
32.   while (1) {
    
33.     CNY70();
    ; d" |" s6 Z# I  s; W
34. 
    . t7 p2 N' l( z" l, @) O- h4 v
35.     int error = ((int)values);
    4 R6 f8 t* ~2 \# o% i
36.     interror += error;
    
37.     int lasterror = error - olderror;
    9 t9 `( y9 ]7 d+ S
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    $ Q) x$ Z8 g8 |5 N: f
42.       power = MotoSpeed;
    - J) Q. W, @: e) O. _
43.     if (power < -MotoSpeed)
    0 R! X. I8 A* f0 l
44.       power = -MotoSpeed;
    5 r) F9 T/ G% v
45. 
    ! t* w. Q1 ?8 ~3 ~9 A
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ' W* w: L5 }! r
48.     else
    8 U3 ?3 N! s7 y( m8 {! E
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    7 _1 Y' q; g4 \  h3 d
51. }

複製代碼

& R: Z- C4 [$ j4 N6 ~

感謝分享

感謝分享