PID 循跡自走車範例

1. #include <Arduino.h>
   4 m5 ~% ]$ {2 Z/ c
2. #include <Wire.h>
   1 f5 W( f! k# U1 A$ z$ W  _- c
3. #include <Servo.h>
   , S0 i$ Y# T7 k( o% A
4. 
   1 \* {0 g! T/ g7 i
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   % K* S+ X! U; g% @2 Z
7. #include "MeDCMotor.h"
   - ]4 d; u4 D  `7 M
8. 
   # ]  F- v7 E5 k# b4 E( U. G
9. //double Input, Output ;
   ! M$ v$ U) V5 Q% q- V4 R
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    1 N/ i' F0 B( ^! s
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    8 u6 @7 p+ C6 |) b: p4 T! L
14. 
    # w: R7 B" |$ \
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    8 P; A( B8 O+ d6 Q9 S' I, n1 y
17. float Ki=0.000201;
    1 k/ F1 c# z! Y8 \
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    6 r0 M7 \, H* v7 @8 x. ]
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    8 j, t9 e7 c+ R0 l& ?4 W
25. const long interval = 1;
    . _& G; G+ K# H& L8 ]# B4 }
26. 
    " F1 N7 s. p# t& N& Y9 y! M: y
27. void setup(){
    1 P( N: k) l; \
28.     lightsensor_6.dWrite1(1);
    ; D% g$ F( u/ [# \# @* R% }, [6 D/ H
29.     nPower = 160;
    6 _* [* r1 X8 I, ^+ m
30.     Error=0;
    $ e& B3 Y$ g3 J& h" R
31.     ErrorAcc=0;
    # r$ |' G) k* m# s  `  m8 \
32. }
    
33. 
    ! R* C' Q' `) D' a6 P$ y
34. void loop(){
    ) Y+ w% C' `5 K3 Q3 ]5 K- l
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    / B: y  O7 s- l& ~& T: r9 _; r. w
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    2 |, U/ X0 F; c5 N
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    4 g1 Z: W! h" N  }, W( s7 f5 k! }
44.       }
    
45.     } else{
    & m- Z$ ]4 Z, V/ N4 _- y
46.       if(nPower > MinPower){
    2 g5 T; f: |: t' o
47.         nPower -= 2;
    
48.       }
    : y. ^) n. R# F4 n; i
49.     }
    % |7 R. j, n8 Z. z
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    + k0 Q" m5 t  L
54.     motor_10.run(0);
    7 |6 c- w- y# \- ]) F
55.   }
    
56.   do{}while(millis() - StartTime < interval);
      A4 `' l# i" @
57. }
    
58. 
    
59. void MotoL(int Power){
    7 p  [' X; r/ F' H4 P
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    ! G$ W& p; a# P# |9 ?8 u
63.   if (Power < -MaxSpeed){
    7 |* W& j# y6 ~+ R
64.      Power = -MaxSpeed;
    ; _* a. `4 t' l' |4 O7 j
65.   }
    9 m/ C7 e) H6 I. [
66.   motor_9.run(Power);
    
67. }  
    
68. 
    + V; v+ u  B8 D* k: a$ F1 ^* L
69. void MotoR(int Power){
    ) C  {5 r# u* r! e6 h; l+ C
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    % M: Y1 z) v8 m7 Y, y4 b. y) v9 h4 ^
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

) y4 O7 M/ g- a2 I" j

4 S5 N9 h: X$ W4 s9 A; u0 k
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

6 z6 W9 o. T5 A9 _2 Z% p但基於想追求更精簡的程式所以還是想請問一下
1 J9 M5 L* x# w; b3 n# \就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
1 E$ Z- g! n& h2 T不知道您是否願意教我可以如何處理
3 c& d8 f, |( ?  x" _9 d
7 ^6 T. N3 }, m' y9 ~/ U7 D
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   ! M5 l$ q0 w6 a6 u$ R
4. int olderror = 0;
   % |: ]% m% {5 G* v0 b% X3 }
5. double values;
   
6. 
   4 Y+ E. B- h8 z! C# N! P9 [
7. 
   5 i8 R. G; c* B8 b  D7 K
8. void CNY70()
   0 s& v+ s/ y% n* z0 k
9. {
   
10.   valuesRR = analogRead(RR)
    9 |9 H  U$ a3 U' e, B+ _2 |) x1 h3 \
11.   valuesMR = analogRead(MR);
    7 U! f2 n6 H: h8 J
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    5 ?& V+ U9 t( w
14.   valuesLL = analogRead(LL);
    & ^  D4 T9 }0 @- ?) Q0 M8 i  Y' ?
15. 
    8 M6 u) @3 T  u& T  D0 d
16.   if (valuesRR > CNY70Val)
    $ Y# I9 G- n5 a6 v/ Q
17.     valuesRR = CNY70Val;
    2 H$ i  }# o8 A
18.   if (valuesMR > CNY70Val)
    ' T) n3 ]: j6 `# C
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    ' f4 E7 J5 N. a8 E2 N! N
23.     valuesML = CNY70Val;
    * J, [+ Y0 I1 J8 S8 l9 h4 V
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    % R! ^( |$ A# B3 c; D  ]( V
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    & g3 J. B5 L" P, o
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    0 A, D$ `+ M( S7 B0 y
36.     interror += error;
    
37.     int lasterror = error - olderror;
    5 `$ R: |, O# Q1 r
38.     olderror = error;
    5 U, l' ?7 h$ e9 d6 p# I
39.     int power = error / 5 + interror / 10000 + lasterror;
    - j! V2 Q8 S' g* B* x7 m
40. 
    ( u& }" x; L' q1 t; m
41.     if (power > MotoSpeed)
    6 f' c( v& F8 v1 _" ?! \# s
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    % r5 m! c3 K4 I
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    7 S- T- u4 I6 Z+ h% E' z+ {
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    3 A9 z# n% b+ J4 d; I3 }* v8 A
50.   }
    
51. }

複製代碼

  W+ D6 s' ^" s; O2 e7 v

感謝分享

感謝分享