PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   ' u; r. @8 o% ~, w1 M# i
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   - W+ a: t! f+ p+ S% s
6. #include "MeUltrasonic.h"
   , r. V/ y  o+ @9 h# i
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    / y3 j' y/ Z6 j8 u' q. O
11. float MaxPower = 180;
    
12. float MinPower = 120;
    & ]; M2 W* v2 j/ N7 {6 F2 z
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    + z2 ^# v' Y0 V- t
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    . j* r" i; d; v4 B% m+ S8 F& T6 c
18. 
    , C+ X3 p# _( k9 o* @8 R
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    - t* l# z; G2 I6 L
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    * u/ e. }5 K6 b
25. const long interval = 1;
    3 s8 [4 S$ j; i9 L$ K
26. 
    7 O3 z, N8 A- t! A! X4 @0 b3 V4 I
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    9 N* v- h3 K" d9 e
30.     Error=0;
    
31.     ErrorAcc=0;
    3 Z9 V" Q3 O3 v3 e7 S- ?) _
32. }
    5 C7 m% _% l! ]9 P& H& C5 W
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    * R# Q8 g( F5 f* D" M1 d8 W0 P8 p
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    % X. n) V+ B( r  v# d1 M
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    / S) x3 C- C3 X+ M
46.       if(nPower > MinPower){
    6 Y, H1 z( r; A
47.         nPower -= 2;
    
48.       }
    7 U* M0 p7 ?6 T% W8 Q% `# C
49.     }
    * ~5 c+ `; E; @% l0 _( C1 O
50.     MotoL(nPower-Error);
    1 a6 ?5 X( z3 a, ^4 i) N
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    * b( q7 ~& T( ?
55.   }
    ) G$ }: m2 x8 F3 C6 U% B( D% L" l
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    9 l$ j# M; R* x) m1 A
59. void MotoL(int Power){
    , v/ n* X* O# z2 o+ X
60.   if (Power > MaxSpeed){
    / T+ d( T( O) q# o
61.      Power = MaxSpeed;
    % T, Q1 G2 P6 V" r0 \  N
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    & p4 M  v8 D- S; P
77. }  

複製代碼

" |( z8 ]; N) |. ]% ^) L

. ^# }5 P# x$ v- }9 s# d
3 r! D! x1 ]3 X/ g' {) B, W6 _" ~您好，不知是否能向您請教。
! M$ E2 ~8 I3 ~: A目前和宋修賢老師在處理Ardui Car
% c' `2 A6 R& G" Q( C7 V& J7 u0 [雖然已使用較繁雜的方式處理了跑出黑線外的狀況
) \  k5 |: X0 C" F& R' n
! c5 U) }+ ?4 P) J1 O) J5 v但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
* \. s* e! _& b3 {" L6 V不知道您是否願意教我可以如何處理


. }' j2 H/ ]- w, ]以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ! {. P- f1 W6 _% \
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   ' a  Z- ^0 C0 L/ I& R
8. void CNY70()
   3 R0 W5 T5 ?$ E0 n
9. {
   % n+ L  M+ p- \1 E4 ]$ P
10.   valuesRR = analogRead(RR)
    8 L! g3 I. d$ T7 ]0 p
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    2 O6 O# `( b" c5 c
15. 
    1 H/ H$ }6 s) q. Q5 r0 w6 G) G
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    # G) ]/ d* u7 w" c
19.     valuesMR = CNY70Val;
    1 B, H% S( H! t' }4 j+ H
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    - Q' I/ X+ i5 t7 |. q
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    / K8 |6 E. D2 w" P4 T$ j* u! {* L9 v; m
26. 
    9 W6 e  Z  W1 m& s* w
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 p6 W! c, w* a3 S8 j! W- u6 S
28. }
    
29. 
    
30. void Car()
    3 y7 j7 ~+ [+ _* F; {
31. {
    5 F) _& u8 v" A8 O- N
32.   while (1) {
    
33.     CNY70();
    
34. 
    % F6 a$ U! d0 i- v+ X0 l9 m' Z' V
35.     int error = ((int)values);
    6 V( D- Z4 e5 b
36.     interror += error;
    1 G" r) p* V4 e2 T" N" d+ e
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    7 ~& J0 C  n# T. b1 |  }: k/ F
39.     int power = error / 5 + interror / 10000 + lasterror;
    6 v# U; t0 }8 H) D
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    * j" W, L# C. j7 U  U2 k
44.       power = -MotoSpeed;
    & A' E0 `1 x& N1 k& T2 X
45. 
    
46.     if (power > 0)
    7 N* J. I! y, S( P: o+ A7 v
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    - W5 P" X4 p! f# Q
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

感謝分享

感謝分享