PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   # F. g# j7 v8 n$ d, C  d! e; a
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   - O4 q$ z) g/ U1 g* j0 G  @
8. 
   1 [/ q2 a% b; e
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    . f) m% ]. L) V: q) x0 W: F1 R6 b
11. float MaxPower = 180;
    1 h' T( ~( e, r+ B* N# q7 J% H3 _
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    1 U. ~# |2 _+ {. {* u* P9 E1 {
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    4 z% W; l) ?) L9 Y- H0 c
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    $ x' W5 k' v# m1 Y9 S0 O
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    7 a5 U' d& O. N5 i/ f! {
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    8 z, U* E7 t5 M4 m
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    6 w# v4 a0 I  {, `  y
33. 
    : x' N6 w2 y, {5 S" ~2 e- M! {
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . d6 {2 s* H3 z0 w
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    2 q. ]# W" m1 n+ j) t: }7 e) d, X
38.     ErrorAcc +=  nError*Kd ;
    8 G1 @9 Y- o8 o: f8 K' M/ Y
39.     ErrorDec -=  nError*Ki ;
    6 y3 G7 X, A, {* B3 Z  r7 R. e
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    & t' V% Q6 M7 r1 z
43.         nPower += 3;
    
44.       }
    ) T+ b. [9 Q7 u9 o4 t
45.     } else{
    . _! `! o% K0 a; q. Z& i
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    , m+ d+ A& P+ F. c) @
54.     motor_10.run(0);
    . p9 `2 T3 ~- H& `+ K7 g- ~
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ) ]# {" K- a; c; n# N3 q
57. }
    ) E0 k" R3 {, v- L( _" l! Z# z# N
58. 
    7 {) P) I7 Q3 ~, ]( }* q" x" q
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    7 R9 g; O5 \, \) j/ e( n/ K( u
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    # P7 H7 k+ q) k) u& F/ U- D
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    $ P- h7 S) {* Z7 |
68. 
    0 Z( z3 P1 u. h
69. void MotoR(int Power){
    ' w% }) p8 J9 M( h" j- \1 \, d
70.   if (Power > MaxSpeed){
    + L. c# x& `4 P' T- w0 Q, V+ M2 ?
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    + {4 G! g# I5 R) }' k
75.   }
    % \4 I6 N8 l7 {' v2 Y" l0 i% ?2 L
76.   motor_10.run(Power);
    ; }  m+ f9 g* }; G. E
77. }  

複製代碼

6 l8 p8 {8 {) A2 s4 U% f& K6 `) d

  o" p/ s0 k  E
: M8 F# y# o/ b  K您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
0 A3 P+ {( H, Y7 L
但基於想追求更精簡的程式所以還是想請問一下
/ N: Q& d: P" `就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

1 @5 @$ `( X- [0 T& C9 {
/ [7 f; h# w- I% y7 z2 m7 l- J6 H9 f以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   # A! y# ?& \7 x# v- s7 F
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   3 [6 b6 m2 v# W! U
8. void CNY70()
   
9. {
   ; r  m, ]) A# |$ G- h. @: u; V
10.   valuesRR = analogRead(RR)
    0 Q) h$ q( j) p" D7 e1 u- H9 \
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    # t; Z) g% d( @4 G
13.   valuesML = analogRead(ML);
    5 G( Y' Y, O% ?+ }
14.   valuesLL = analogRead(LL);
    4 C- I9 B  r. U7 @
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    $ l, x0 s: x3 n' x6 J8 H
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    6 H$ _; D2 h6 H3 ^
24.   if (valuesLL > CNY70Val)
    8 C% `3 z$ e. g# q8 B+ V
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    % j  h) f2 M. |0 u+ n$ a. D
30. void Car()
    ' x% r7 [7 p- D" \& a
31. {
    
32.   while (1) {
    
33.     CNY70();
    % x2 H9 k. b! o  g3 y: H7 j  R" @
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    . x, O" Y% K7 d
39.     int power = error / 5 + interror / 10000 + lasterror;
    + ~. Q1 i% w2 @1 w3 @8 S
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    3 }& u4 J) v8 u- G6 j6 \: |( }
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    4 e" v, r1 v; l
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    # c. ]# u9 f0 x7 u5 |2 G% P
49.       Speed(MotoSpeed + power, MotoSpeed);
    ' V# A# H/ C% \* R1 ]
50.   }
    
51. }

複製代碼

3 ~' b) L1 g2 ?9 f- x
$ {- V8 r- c7 [

感謝分享

感謝分享