PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   ! c+ y- e$ T" _, P
4. 
   / d- n; H8 C0 f# a( K
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   ( g& |( ~0 q8 X6 [5 m
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ) c0 |: Q. D" k! P! X3 U1 g
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    8 P4 T9 r4 D( X! S" N7 Z8 Y
15. float Kp=0.14;
    ( R! G3 D. O# f+ F& |% a2 g/ S. \
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    2 B9 ~) G& U! \) o
19. float nPower;
    
20. MePort lightsensor_6(6);
    : o8 B) f2 {& [" K
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    * s) Y7 ?& D1 P( b* R
23. MeDCMotor motor_10(10);
    - I' R0 i5 \5 H7 L, Q$ B+ Y
24. unsigned long previousMillis = 0;
    ; ?- a  t; h) b& u7 d  k7 v# \
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    " U8 b' f* G% J5 z. C1 t8 m' w, j
30.     Error=0;
    / B: D% C4 U( K& l
31.     ErrorAcc=0;
    
32. }
    5 G/ k4 ?. q( i- L
33. 
    ; e- i/ F! ?( u, s6 _( T9 W+ N0 M
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    % m2 f# O) p* u7 X
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    6 ~& {- k2 \: ?* w2 k) ]& t
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    6 ?# o) y' `$ }5 Y5 h0 K& `* e8 C
48.       }
    
49.     }
    : R7 ^; U; m/ E* W
50.     MotoL(nPower-Error);
    7 [+ g, w/ _% Q
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    . r. c0 X3 ~! f; B9 q, ]+ {
57. }
    
58. 
    ' [* N! T/ X7 l
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    . u, @+ ^" E; C' H2 c
63.   if (Power < -MaxSpeed){
    & R7 f' G& x* q; G$ F
64.      Power = -MaxSpeed;
    
65.   }
    ( [" ]+ k1 I. X# S+ z" T* U' I3 ~" d
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    1 m' t/ `( L$ L4 W0 b1 O3 H
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

2 y' C7 v1 P' Y/ ]- I& J( y
& ~1 C5 F1 Q& Q9 V! `您好，不知是否能向您請教。
  I$ T/ Z( J; T6 N' g& N目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

5 G+ j8 Y5 f/ T5 c+ a; X! Q5 M但基於想追求更精簡的程式所以還是想請問一下
) X1 I0 M4 J8 q% w, n* v就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
9 X* B) a1 g! b% a不知道您是否願意教我可以如何處理

8 W" r! X8 }  G+ `
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   7 y5 B! D+ y5 A$ R/ C) e
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   ! y' T6 n# V3 n& i+ v$ m
5. double values;
   , e0 n3 R$ |, ^' W5 Y+ Y( O
6. 
   
7. 
   
8. void CNY70()
   
9. {
   * \  M" y& _# K. {
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    2 i  h# h! l" u3 y. @  o4 p
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
      T$ T7 m) v+ g* f
15. 
    
16.   if (valuesRR > CNY70Val)
    ) s0 K8 a' Z' o7 \  K9 q* s7 j8 T2 ?
17.     valuesRR = CNY70Val;
    . o3 _- c$ e. F" |8 Z+ @& y" m
18.   if (valuesMR > CNY70Val)
    ) a& f; C+ @. F! C8 Q' O$ n9 h; _
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    2 g" d$ k5 T7 e9 d- Z  ?6 U& v& a
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    + n6 s1 b) a4 Y  s5 A7 K3 _- H) V! I
26. 
    4 }  g3 _7 A4 D5 a1 F+ l. T
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    9 t& M$ i; d! j. S7 L/ G
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    4 C) @4 H, d* _/ q& K; @% z" `
35.     int error = ((int)values);
    ; D5 i$ }  X9 B
36.     interror += error;
    3 p$ a2 c, E5 S+ q1 q
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    / |6 J& i, j( R+ B- z7 }- y. ?
41.     if (power > MotoSpeed)
    ( m& {2 a' R( ]3 I! Y0 s
42.       power = MotoSpeed;
    $ P, I# y6 f+ N9 G
43.     if (power < -MotoSpeed)
    5 `5 A. H3 D" y, X. b* X1 d5 N
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    & [" H! O) u7 o7 w
51. }

複製代碼

9 S8 q( N5 W- r
$ R, z- `& v+ g5 u6 G

感謝分享

感謝分享