PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   ' r' A1 u7 J8 F* w
3. #include <Servo.h>
   - B8 `' s! f; M$ G: D
4. 
   1 ?8 N& k7 h: }8 _  L
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   6 J6 N2 s! T$ Y/ j/ t  n3 [. R% a
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    6 a+ C2 p$ s! Z* Q% J4 k& F1 T0 ?
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    6 s5 g6 ~* ^0 y
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ; F5 M9 i* k) Z2 R3 e8 j+ x
23. MeDCMotor motor_10(10);
    + ~( S5 q; ~' L3 x' h- b
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    - E6 D- `; ]( G" l, i9 F1 N+ i
26. 
    1 b  d$ ^# _/ ~0 d) [  F9 ~
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    & ~9 i3 V2 X1 R2 t% F7 w
30.     Error=0;
    % S) w  s$ U  a6 T, W, _4 e4 g6 M
31.     ErrorAcc=0;
    
32. }
    
33. 
    
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 ;
    8 L0 Q) D% h$ d
39.     ErrorDec -=  nError*Ki ;
    1 Y( ^0 _0 k  p2 i$ a6 S6 [, Y
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    & I8 Z; X& W; i$ C0 B* M# y: Q8 y& T4 [
43.         nPower += 3;
    # N! d% S7 o1 ~4 V& d
44.       }
    4 I6 i( r$ L) r! `# b1 |. H6 R2 C
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    # N+ c+ d: {5 r/ O
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    7 c1 g( D6 A, y; c! R& H
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    2 W3 C3 ?" O. j% v  j
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    5 V+ [2 C4 L( K5 e9 }4 x2 Z& D
57. }
    ( q4 y8 A) R% Y- l5 `  a
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    7 h# A/ }, @. Z* R3 a
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    , j9 f; y' t5 V# M9 o2 R
64.      Power = -MaxSpeed;
    * R( q7 r, n+ T$ s& z$ h- X& w
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    9 l( v! z/ L: ^# _4 B' }4 R
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    : ?( y/ {- F6 U3 I( c
71.      Power = MaxSpeed;
    / Y" K* B" z* R' B& l
72.   }
    : H: z# `) N; _/ S4 x" f
73.   if (Power < -MaxSpeed){
    & f2 m/ P) W7 \& t8 F7 w( f
74.      Power = -MaxSpeed;
    ) u" a0 p3 k) ?9 B7 K
75.   }
    
76.   motor_10.run(Power);
    : B' n+ V( m, I% t) L! X
77. }  

複製代碼

" X. K: }, S/ |8 s' P: b! n% e

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
: F  k1 a: _7 ^; d* g雖然已使用較繁雜的方式處理了跑出黑線外的狀況

1 M$ u0 F" W# E% ?: p但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

: M4 {# Y3 d9 C- I1 l' Z
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   0 g1 M0 B, b5 W7 s8 o
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   9 P. t1 b4 c$ g
7. 
   
8. void CNY70()
   : [, \9 E* a2 Z( A
9. {
   
10.   valuesRR = analogRead(RR)
    : ^0 @+ N4 }  F6 i
11.   valuesMR = analogRead(MR);
    2 c" h, `8 ^& ?8 U& n4 n) d0 l
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
      t1 d" p) B' I( q, r3 T9 x
15. 
    
16.   if (valuesRR > CNY70Val)
    5 O+ k2 t" _) Q  ~9 W- [
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    2 ?% s- X/ F1 j. ?
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    ' a8 ^" }) r; G8 g* s$ W
23.     valuesML = CNY70Val;
    * c. y4 n( B2 I6 b- h% K
24.   if (valuesLL > CNY70Val)
    1 u( d- A: }  q: O
25.     valuesLL = CNY70Val;
    " Z+ D/ q3 F; ^
26. 
    - M. h* o$ r9 \: U* \; I5 g
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    2 Q0 R3 F3 I- E' @& @
36.     interror += error;
    ( U- p$ C9 O3 ]( \9 Y9 v+ @
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    ) {# J+ d+ ?' T; R: R5 V. c: M2 a
39.     int power = error / 5 + interror / 10000 + lasterror;
    ' Z0 {/ u$ u5 ^1 x/ x5 h
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    ) L. {$ N  S* Y8 H
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    3 g' `$ V8 q0 Z- E1 n! ?+ H. \
45. 
      r3 B# k: Q5 S( d" N% D5 X
46.     if (power > 0)
    - m9 V+ C3 b* X3 J
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    . Q0 q! X$ ?- M5 m$ G0 I+ A/ s3 D
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

感謝分享

感謝分享