PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   / r3 C9 L/ v, P8 u
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   8 ?( Z9 t' Z5 j0 b: F. G7 u7 ?7 H
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ' X; G( l. n, u: l0 D
14. 
    & |  H  c: c9 L8 C9 D5 [
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    1 l! `1 Y4 M  }
17. float Ki=0.000201;
    
18. 
    . w. P2 w! ]( P
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    6 V5 [# [" V7 p' l
22. MeDCMotor motor_9(9);
    " a2 a  m! ]+ u. L- Q$ M9 i# ?
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    7 j+ Y; A0 x! [7 u
29.     nPower = 160;
    
30.     Error=0;
    6 m- x* s* i* v  |
31.     ErrorAcc=0;
    ' {, v2 b* h3 p. C: K9 g& K. g. Z
32. }
    $ Y- s1 k! G7 p6 i5 {
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    / J& b0 d$ q, P6 I: t' t4 E/ `
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    / G1 ?" U1 \6 q
41.     if(nError < 80 && nError > -80){
    & h- W7 L7 c6 f8 V( r
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    * g3 L5 V- W) d7 t
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    $ r9 y5 _& f5 F8 n; y
48.       }
    " K3 \. Z! a9 a- i5 Z1 _
49.     }
    : B- d1 \2 p( Y
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    4 L5 h: k9 a3 d
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    1 L' r( ]% |4 }6 o! U9 Z# q" c
57. }
    - ^7 K3 F9 J3 h
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    / q; P& r' K2 X* t5 `: J: u6 k* `
63.   if (Power < -MaxSpeed){
    + Y' u/ {, |3 W1 m9 e' b
64.      Power = -MaxSpeed;
    
65.   }
    ' q( M8 f0 P2 E  d" P+ U: k
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    : o/ j% W% T) O
71.      Power = MaxSpeed;
    % o# Y9 }$ Q! R" `
72.   }
    - R/ A# d7 P- _. J1 I
73.   if (Power < -MaxSpeed){
    / |3 }4 ]2 D' @) H
74.      Power = -MaxSpeed;
    ( o1 v- a( ~5 ]8 P8 ^7 x( z
75.   }
    7 ]( l; z$ W7 c! s
76.   motor_10.run(Power);
      \& o- a% R0 d+ o
77. }  

複製代碼

4 X/ ~, o8 z) n8 o
7 ]6 B  x7 E# q2 b  J" b8 Q0 s( X. j

* H+ N5 C/ c) Z. M: Q
您好，不知是否能向您請教。
7 z0 E1 |7 v0 S- ~7 p* b9 A  |. E. N目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
7 f6 {, x  o# B* J不知道您是否願意教我可以如何處理


" d/ Z3 v/ a2 r% B; O以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   2 h$ q5 y8 x2 Y) x% m2 Y
2. double CNY70Val = 1000;
   6 y  H8 a. _3 @1 ~
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   5 k$ O# i' M% R5 e4 Z8 @# M
6. 
   
7. 
   
8. void CNY70()
   4 i9 y0 s2 ]- f9 V( j/ m, y6 r
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    9 P$ \( M% y, x
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    4 S# `7 N6 f. f7 }# w4 T% w1 |# @
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    : H; t/ _' L, L  w# y
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    % |4 @$ H% b5 d5 r. E3 F
22.   if (valuesML > CNY70Val)
    " F! @( z. d1 `6 S) b) s, e1 L! r
23.     valuesML = CNY70Val;
    + O" Q$ _, F6 F4 k
24.   if (valuesLL > CNY70Val)
    + f* ~8 l) B5 v# w* z0 t0 `
25.     valuesLL = CNY70Val;
    - N5 ?/ P+ p6 @1 @# K
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ; W2 W) `4 u: ^
28. }
    / _- o. u) v, u3 K  f$ W) M
29. 
    ' U) C" l" l) j
30. void Car()
    8 Q0 b# E, z( i, p1 Z
31. {
    
32.   while (1) {
    
33.     CNY70();
    $ k! d% z( t. O. I
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    3 }* p: C# V% S. H- W$ p
37.     int lasterror = error - olderror;
    ! ~6 D9 w; c7 ]4 s! b
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ; @9 S/ e2 M! N) B+ g- m& W' y
44.       power = -MotoSpeed;
    1 o: d/ s; r0 W: N. C( x( g
45. 
    0 W9 A1 [) U  v& m: B
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    2 |( `7 {3 k; D, b9 v
48.     else
    3 I$ h* k7 z) r9 ^
49.       Speed(MotoSpeed + power, MotoSpeed);
    . @$ l" B& H: y2 t
50.   }
    
51. }

複製代碼

6 g) J: g/ I  H' r3 k( v

感謝分享

感謝分享