圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21859|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>0 M; l0 I2 H( }
  2. #include <Wire.h>- {$ V( \  G2 [
  3. #include <Servo.h>
    ' A7 p) P! {) D8 @0 j

  4. # X( X( Q5 }8 U5 D
  5. #include "MePort.h"
    & h& r5 d+ [7 B7 t5 @9 Y# p
  6. #include "MeUltrasonic.h"
    5 o) C) Z' n9 z
  7. #include "MeDCMotor.h"6 N! T5 a, L5 S* J# y

  8. ' ]3 E+ i+ @5 R- E3 a& @
  9. //double Input, Output ;/ Z8 R; ^! \& B$ z$ o- j0 f
  10. float MaxSpeed = 255;0 t" s& z! O) K9 B: K, i
  11. float MaxPower = 180;
    / l2 R9 f4 k9 V/ H$ J% b3 `; O, C
  12. float MinPower = 120;2 k. R" [. H+ Y5 m) y% B
  13. float Error,ErrorAcc,ErrorDec;
    . I9 N' s7 w. D& k: P/ f
  14. 5 w- r5 P4 _% H8 O7 b) S3 I/ @
  15. float Kp=0.14;
    7 B0 }' }: r' G, N: W
  16. float Kd=0.00020;//23;
    / U4 h4 V: s/ N) b5 j& P
  17. float Ki=0.000201;+ e$ x$ N( d) @' M  ~6 W
  18. 8 L( z; R' R; \5 r6 Q
  19. float nPower;( Z( v$ U2 Z( ?/ T5 D, q
  20. MePort lightsensor_6(6);" p+ p$ Y. Y' P9 ]1 G5 A$ Z! C
  21. MePort lightsensor_8(8);
    , o& e! W$ v, M7 i) Q. S( _' H* K
  22. MeDCMotor motor_9(9);
    4 ?3 C) \' W9 ]1 P/ @2 ]5 X
  23. MeDCMotor motor_10(10);1 S# z& Q" [; }1 w+ N
  24. unsigned long previousMillis = 0;. t. p" x. H6 B
  25. const long interval = 1;
    : a, ^( y! O" U; y& E
  26. 2 {- z3 C$ h3 |/ i/ m5 \
  27. void setup(){; q$ P+ l, J& B7 s
  28.     lightsensor_6.dWrite1(1);( g; U8 t6 W4 N/ Y1 D1 P
  29.     nPower = 160;
    - ^6 ^+ x3 `9 `) B: L8 B
  30.     Error=0;
    9 [+ m4 t& y0 [* H7 v- \% B$ }
  31.     ErrorAcc=0;$ [# I, N6 z9 ~# f3 a8 M
  32. }3 n. `; X' G. h; U% j3 B+ l% L8 g& v

  33. 5 o& C0 A& A" Z) |0 u. v" z/ }* h! o
  34. void loop(){
    9 ]/ S$ @: c# W4 S
  35.   unsigned long StartTime = millis();) G7 r2 a# U! L$ B
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){0 X' g% Y1 _4 i% j; I. Z0 s
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();0 G* S8 }+ W* U6 Q5 y
  38.     ErrorAcc +=  nError*Kd ;7 W+ d0 `1 t9 ~9 C- g9 [7 a
  39.     ErrorDec -=  nError*Ki ;
    - B1 E2 x: X! d! O# q- ~* N
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);: c/ X) Q. O9 R9 ^! v& p
  41.     if(nError < 80 && nError > -80){6 U- v3 s% T- ~( h3 e5 ?3 Y
  42.       if(nPower < MaxPower){
    3 ~0 Q2 h8 v+ Y/ h
  43.         nPower += 3;) I- o& g* g. u$ G# x
  44.       }
    4 L* h' n/ P  k/ X
  45.     } else{
    * G' s. i6 |+ n3 ]* N8 C6 ^5 y
  46.       if(nPower > MinPower){
    5 v' v7 M0 O: I# x9 T! y' H
  47.         nPower -= 2;( Q. |7 ^5 E% m  ^1 o9 ^
  48.       }& F9 N9 }, E$ P8 v
  49.     } / P0 q9 J5 j8 Q/ T+ u
  50.     MotoL(nPower-Error);
    3 {  w' F+ Y7 a' q$ D3 J
  51.     MotoR(nPower+Error);    : _& @+ h+ {. H" Y
  52.   }else{
    / Y9 Z5 r4 c! U7 \2 P" m
  53.     motor_9.run(0);
    ( Q, t: E( ~( V/ q3 D6 Y- ?& g; _6 b
  54.     motor_10.run(0);7 D; o5 O1 T7 G
  55.   }3 N0 m: A6 W" J, o
  56.   do{}while(millis() - StartTime < interval);& ^  x8 o9 `" t; z9 o# O  ~( A
  57. }
    / [( Q! A4 H$ @" m1 q! t7 Z

  58. # L5 i" V7 U# U; h9 i: n& [! o
  59. void MotoL(int Power){
    ! d+ h) z/ ~  |8 R
  60.   if (Power > MaxSpeed){
    ( h* O5 H+ Q; h4 y6 ], A* @
  61.      Power = MaxSpeed;
    $ g; A! a, B, `
  62.   } % z; L- h6 v) L. p9 ]8 a: Y
  63.   if (Power < -MaxSpeed){5 x6 z( Q4 C+ _5 X
  64.      Power = -MaxSpeed;
    / Z) M2 I# I$ w- _
  65.   }
    - a7 B# O* ^1 t9 C4 k5 J% w! x
  66.   motor_9.run(Power);$ z2 f% T: o0 ]7 s
  67. }  
    # Z" g4 S- x, M3 I* n

  68. 9 T' Y5 l1 q8 W
  69. void MotoR(int Power){
    * W; b# X# h8 ~  \7 B
  70.   if (Power > MaxSpeed){2 ?) U7 y* i9 O2 Z1 [
  71.      Power = MaxSpeed;, ^) ?. M" b# D* |9 |
  72.   } * I( p) S1 j; r' E: r
  73.   if (Power < -MaxSpeed){
    4 S  w% b. T5 ]- q) X1 S
  74.      Power = -MaxSpeed;$ ]( I- W; V- l
  75.   }
    ' Y# w* Z  d. p' J3 M2 \# g
  76.   motor_10.run(Power);
    ( t5 r/ d2 I' G$ C( V0 S! f  J
  77. }  
複製代碼
  ]  g" ~# V# p0 K8 q
! J7 I+ @) J9 c0 I; \
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 - h1 z% `* h- R: Z' S( m

; ]' d8 _4 Q, Q4 u, e8 {9 e% f您好,不知是否能向您請教。' J: g/ U9 Q/ K3 v0 |# S9 Q
目前和宋修賢老師在處理Ardui Car/ @" }8 J3 m( x/ P/ h; i  f
雖然已使用較繁雜的方式處理了跑出黑線外的狀況: w5 y- W, c  h$ l5 z# X3 t
; g5 T7 c8 Y0 {4 Z9 j
但基於想追求更精簡的程式所以還是想請問一下% ^: p% \5 K( D! H( z7 ^, H- h
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
6 T4 N; Q( w7 k6 \不知道您是否願意教我可以如何處理5 \2 O. V' v! X! H: B6 g: U9 ?

7 L7 Z& `  Q. q% f- h
- Z% E# ^: j4 |; B/ v$ ?+ N以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;! R' r: z7 m& }- V9 x8 e5 x
  2. double CNY70Val = 1000;
    9 M! @3 V* e: |8 n
  3. int interror = 0;3 }0 b6 v+ X1 ~6 D7 U4 U6 U  A
  4. int olderror = 0;/ @; h: j2 \& B3 v$ B# Q
  5. double values;+ O3 H  b5 @2 W4 x/ m. p3 r
  6. # r% U* `. U: [; @+ l: ~
  7. 8 a+ T" B' H- a2 P( Z1 m
  8. void CNY70()
    ! C& R$ F5 N2 Z' C: g
  9. {' x1 |. u  K! }2 B1 m- N: S/ k/ ]4 X
  10.   valuesRR = analogRead(RR)
    0 q3 g* Y( K; a) g/ f5 a# q
  11.   valuesMR = analogRead(MR);
    % r3 d$ R& V! ~
  12.   valuesMM = analogRead(MM);! r! I- s- E% s$ P8 \; ]
  13.   valuesML = analogRead(ML);' ^9 i6 s  D4 b6 \0 _! P' n
  14.   valuesLL = analogRead(LL);
    1 X' Z, Z6 Y; u/ u
  15. ; P0 r# w) M  D5 b6 A' T6 U; L' k
  16.   if (valuesRR > CNY70Val)
    : e2 K2 I2 @2 [' V+ X
  17.     valuesRR = CNY70Val;
    # d5 l; S& ~' `* ?% J# Q
  18.   if (valuesMR > CNY70Val)
    8 E* ]! W' R% A6 D1 O8 _+ I
  19.     valuesMR = CNY70Val;
    ( C9 V, P* c8 a
  20.   if (valuesMM > CNY70Val)0 P" C' k7 k  A+ g. n
  21.     valuesMM = CNY70Val;
    2 h# d1 v* i* c) L2 @, s' E7 T3 r
  22.   if (valuesML > CNY70Val)
      z5 {2 W: Q6 c/ s$ @/ j( E. j
  23.     valuesML = CNY70Val;( m3 M, @' S" i' F
  24.   if (valuesLL > CNY70Val)% f  H9 w4 z" v. l
  25.     valuesLL = CNY70Val;8 b6 K3 Z: J5 o% c0 \( B

  26. * a1 P. S9 b1 |9 ?% W
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;1 k. z9 x0 o: x$ y
  28. }
    1 b6 _: g3 k9 J  X# r

  29. ; U! K3 _( t% \% {0 k$ d0 h
  30. void Car()% M' U* i) {2 ]
  31. {  v. S. w2 b5 C/ l, d
  32.   while (1) {0 `" {; |8 G1 y' l
  33.     CNY70();3 w/ \& M$ S1 S& v; M

  34. ( C) m8 l& g2 I: D/ a$ s
  35.     int error = ((int)values);
    $ f0 _, j$ i0 o# @% a& s
  36.     interror += error;
    ! ?& \9 [! g& B: @& D0 p! n
  37.     int lasterror = error - olderror;
    " \; N2 b0 A2 z, ?0 z) W. f
  38.     olderror = error;% i1 t: `+ L* R. N7 k
  39.     int power = error / 5 + interror / 10000 + lasterror;! D4 W4 {# E- G6 s' Y% ~* ?* }
  40. ; E% D7 s4 t$ d* n1 X; R
  41.     if (power > MotoSpeed)
    % S3 W8 b7 [; }$ l5 }9 s2 V7 ]) @
  42.       power = MotoSpeed;, b7 W4 [8 [$ W+ k
  43.     if (power < -MotoSpeed)
      C* J4 R6 _& V7 A% V- r9 {
  44.       power = -MotoSpeed;
    4 J+ Q" R7 V, j+ n+ {
  45. ' y8 M2 ]  D) y9 m3 u
  46.     if (power > 0)* K# L$ i. C/ d: {3 M
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    " y8 e! J* V2 T$ f. ?4 W4 _
  48.     else" E! z+ X( v, {4 l* {
  49.       Speed(MotoSpeed + power, MotoSpeed);
    7 f, V1 G8 H# X4 ]. L) q
  50.   }
    6 _  I0 Q8 i6 `1 q" r
  51. }
複製代碼

5 x* U4 k, J6 U9 ?; w5 N: |2 j7 N+ s4 k0 h. m
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

QQ|Archiver|手機版|小黑屋|圓創力科技有限公司 IOP Robotic Technology Co.,Ltd Tel: 07-3924582 Fax: 07-3924001

GMT+8, 2025-12-6 07:11 , Processed in 0.024385 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回復 返回頂部 返回列表