PID 循跡自走車範例

1. #include <Arduino.h>
   3 r: K5 U( T+ v4 W% x# f
2. #include <Wire.h>
   , c  P1 r0 B3 S* @' Y, p
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   , z" _/ E3 x3 I& `
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   5 Q- L1 B) A9 w1 ~, ^' U
10. float MaxSpeed = 255;
    & q- v# R8 u. T2 A+ ^1 \+ a: A
11. float MaxPower = 180;
    8 D" T- \* d5 R. z, Y
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    $ f% b: I- F! ]$ I% y
14. 
    % |# J, B. o. ^
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);
    4 \- W( s2 K' d! u6 L
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    , }+ ]+ N7 l  e
24. unsigned long previousMillis = 0;
    % ]' l# i7 o9 Q& c+ l( \# S; t3 b
25. const long interval = 1;
    
26. 
    8 H* x( R8 S" {3 J" {4 @
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    7 \+ k/ a5 V3 t% r; O
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    * p9 s1 f% X. }% K. ~
33. 
    
34. void loop(){
    $ q# r+ C: n- U
35.   unsigned long StartTime = millis();
    # Q. n* x) v1 {( U! u. M" @
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    : W' ^. p0 B, \3 x! R! o! r! l3 p
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    9 j  V2 v' g/ {7 h: m. t
43.         nPower += 3;
    
44.       }
    ( Y  M- T( `) L7 e. v- g
45.     } else{
    # \* }4 {4 b* T# T% `1 W, b
46.       if(nPower > MinPower){
    4 d: P' j3 N( P5 E' N
47.         nPower -= 2;
    3 `) _% q1 s+ d& |$ l* m$ N
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    3 K( t7 l8 M/ R* F* C) Y) q. o' ~" F
51.     MotoR(nPower+Error);   
    ; N& r" B! z$ B; o& T9 U8 r  Q
52.   }else{
    % i: H  U5 e5 F' @/ F0 `
53.     motor_9.run(0);
    ' [# y2 `/ j( Z% B. Z
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    - G- j8 E9 c  A  i& ^
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    4 n3 f" H$ Z6 f+ N) |
62.   }
    6 c, J2 y$ e- a4 e- Y7 r0 ?' ~
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    . u2 J9 @( `; V3 w( k' e6 ]- H1 o& o
65.   }
    
66.   motor_9.run(Power);
    8 q- f% I6 L3 _, j% c: \: q% M0 E. r# }4 G. M
67. }  
    ' r; F- w) X/ e7 R, W  D& }1 F0 G0 }2 e
68. 
    
69. void MotoR(int Power){
      }$ ~1 q, ^- e! b4 L
70.   if (Power > MaxSpeed){
    4 y" q. G4 z* w  y* x. @9 ]
71.      Power = MaxSpeed;
    ; t1 B- v/ U: K! s$ L0 H
72.   }
      \5 {" v. K' O$ Z! c; j2 H
73.   if (Power < -MaxSpeed){
    7 O- Q( M1 b- @3 N$ i6 z9 p
74.      Power = -MaxSpeed;
    + L* {& S$ ^+ e: A! n( g
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

# a# G6 ^9 b  |- G
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
; I! d; B& s) p' G雖然已使用較繁雜的方式處理了跑出黑線外的狀況
# f5 V; p* N/ C
但基於想追求更精簡的程式所以還是想請問一下
0 I& @, }/ `6 \& E8 Q& ^$ h就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
+ r8 ^) `; x, ~: n9 C. F1 i不知道您是否願意教我可以如何處理

3 }8 J4 X. g5 c0 N, D2 M3 N3 t
! D; @( v) Y5 D9 s+ _1 K& T以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    + V9 N+ K4 {) @: E* \
11.   valuesMR = analogRead(MR);
    3 R# ^$ |: `) j/ Y) {8 k
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    , z2 N  C  j2 v
14.   valuesLL = analogRead(LL);
    6 A& K9 X" g. D/ @# O  ~
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    . s6 N! J5 z. c1 T
18.   if (valuesMR > CNY70Val)
    2 G% }$ |0 i( ]! _" Y5 T! \
19.     valuesMR = CNY70Val;
    ( U- t; a& U; U, d+ M2 A3 X, T
20.   if (valuesMM > CNY70Val)
    . f) D( g; R( F
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    3 l5 {6 G  W* k9 p& H$ R
23.     valuesML = CNY70Val;
      @0 F8 ~' J+ C) L2 |3 R; Y' o
24.   if (valuesLL > CNY70Val)
    . F! [, W6 |" |% ^
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    5 \# N: W5 h8 j' |. R) q; F# @
28. }
    # F; ]* H9 E. p( E
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    0 {$ E4 j0 h* q0 f, U
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    5 q% m# X; K3 ?- K2 H8 L
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    : H5 Z& t# x6 f( ]
42.       power = MotoSpeed;
    $ q8 h% Z0 ?$ G9 s/ y: {; Q
43.     if (power < -MotoSpeed)
    1 D5 w- [1 N/ t, B3 [
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    1 e) J/ f( [7 [
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    # X$ X1 E, b' I. M5 Q9 y- ?
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    $ |* \# L4 w' d  c4 f1 j
51. }

複製代碼

$ Q+ R5 Z% J7 f6 D7 R! o

感謝分享

感謝分享