PID 循跡自走車範例

1. #include <Arduino.h>
   - o1 p5 Z3 d, Q& I( H( x) Y, F6 a4 c9 C
2. #include <Wire.h>
   
3. #include <Servo.h>
   & G4 _  ?, q0 ?
4. 
   & m2 h8 B8 ]* @- D% e
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    - y) d. Z& ~2 s- ^3 H: B- d
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ( c0 ]; j1 h  X2 a+ L5 A
15. float Kp=0.14;
    * J! d3 E. k: F+ |4 ?
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    ; ]0 u$ P" ?( F8 t/ T
18. 
    ! N! V; y; T+ w' x, j8 z
19. float nPower;
    
20. MePort lightsensor_6(6);
    , E* @" i8 O% k; ~6 ^9 S
21. MePort lightsensor_8(8);
    * E" f1 e; I9 D% f9 p& [
22. MeDCMotor motor_9(9);
    ( Q9 E2 j* _$ y/ \# S8 J
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    ! l' i5 |6 g: z
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    # h# W. h# z  |/ r+ z" z: o
32. }
    
33. 
    ; {. t  w, u& W2 N- s
34. void loop(){
    . |" v! Y/ w/ U4 X( w
35.   unsigned long StartTime = millis();
    & W# K; ?3 m6 [- L/ X2 Q
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ) K  E# T, B% S1 r$ B* b5 c5 y
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    : X4 Z- A  V+ F0 S
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ! U5 K' X/ H# _7 `0 [# |$ i' c
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    ( X, |1 @9 A6 N% G, d9 |! b9 d1 R
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    / p9 e( P) b+ w( R7 S& C2 o
49.     }
    
50.     MotoL(nPower-Error);
    4 H. G, R4 e' ~* ]/ L
51.     MotoR(nPower+Error);   
    . y0 A/ P# g8 c+ @+ J4 W  w
52.   }else{
    6 G( b( ^+ X* [: ^6 E( ~
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    ( a  E* C2 v1 Z7 w5 A- `# n
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    . `+ S0 m% a0 X  C  w  U  U' k5 X
57. }
    
58. 
    - ]& k1 W: V: z+ B) V
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    ; m! {0 z- v- j. d* L& \, w
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    ) `! i0 m# `2 b2 {2 ]  K1 D/ S0 |
67. }  
    ( T# p8 I* `: T# x/ [3 s$ m
68. 
    2 Y5 V* f) ^) S* u( J- j/ O2 G
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    2 t+ V. w3 u  z3 _  N
72.   }
    + ]7 R* E7 `1 u$ c
73.   if (Power < -MaxSpeed){
    : f' p0 e8 u6 ^; a$ T9 K5 \( x
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    ) `% z1 J8 L/ q4 S1 u  i
77. }  

複製代碼

9 X) W5 v% a2 ~4 S5 M* h- o" N9 s

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
; \. h/ }5 C6 U7 _4 e
但基於想追求更精簡的程式所以還是想請問一下
: [5 Q, q) b/ }7 a; N! W* \) X  H就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
) P* c4 a% [' A/ C7 S/ G

. ^0 g# \. d' i以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   1 s' p4 |6 @; W; l
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   : ]* f3 B$ n8 z8 ]
8. void CNY70()
   
9. {
   % w: u" V' H, D" e/ ?+ Y
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    ( B4 {/ T+ q, v
12.   valuesMM = analogRead(MM);
    0 H" e6 x3 e$ r& e; o4 W
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    $ m  g2 N7 Q2 x( U( {# S( P
15. 
    
16.   if (valuesRR > CNY70Val)
    * q+ _5 ]- t, R! z0 S9 u
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    3 P2 Q! {8 ?) J/ }2 I
19.     valuesMR = CNY70Val;
    & a& d* W& w6 |" B
20.   if (valuesMM > CNY70Val)
    8 r( x+ h0 W8 ?' c) F5 a$ Q- T7 T- g
21.     valuesMM = CNY70Val;
    * [+ D. R: U5 f# S: P6 `
22.   if (valuesML > CNY70Val)
    6 b/ X2 M4 l$ H6 U& C" @9 s2 F6 @
23.     valuesML = CNY70Val;
    9 |7 C( k8 l  J$ j, L
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    6 x7 b# U) s8 I- j5 X
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    % U7 |2 c, z7 F% ]' k8 r
29. 
    ; ^9 c/ X: _- {8 l; E) u
30. void Car()
    9 b& L1 u$ u& z; u; a7 b
31. {
    
32.   while (1) {
    + P7 H5 l* j2 {! l3 U
33.     CNY70();
    
34. 
    + s- A4 J1 I& C0 P
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    ( n" {' x) H* }$ @
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    2 _* ?. J- r+ R7 \/ \2 X
41.     if (power > MotoSpeed)
    * `$ u. p6 }' h4 J- |
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    ' t$ f. Z5 Z# A$ K0 h
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    - q% k$ ]* Y+ ?) G
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    2 p1 W: V0 q8 d5 _( @% y
51. }

複製代碼

感謝分享

感謝分享