圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 20965|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>/ n. _9 A2 {$ t# M5 L
  2. #include <Wire.h>  T. B1 Z5 s- s( ^- {! U2 c' B
  3. #include <Servo.h>2 k$ D& }8 q+ S6 L) z; |

  4. . R0 ]0 n# v% g& G; `
  5. #include "MePort.h"1 x% o& m# Y$ A/ h# K6 O5 s
  6. #include "MeUltrasonic.h"
    $ u% J7 }2 }- R" l
  7. #include "MeDCMotor.h": r7 d# ^1 F8 p0 |# G$ P

  8. 8 L9 m) A4 \+ ^1 e4 f1 T0 s# }: D3 B
  9. //double Input, Output ;
    3 ?# F- u& ~) }5 }0 n! A1 O
  10. float MaxSpeed = 255;
    6 E' j( k7 `& G% V
  11. float MaxPower = 180;( G0 K! N' ]1 Y; g/ L! d
  12. float MinPower = 120;
    6 C. l1 K4 }* q* _/ O5 v3 \" t
  13. float Error,ErrorAcc,ErrorDec;. k, f7 n) I# e$ u
  14. * ~- D( ]( ^3 I. ]6 x
  15. float Kp=0.14;
    ' l: u6 z+ n) K- |* b
  16. float Kd=0.00020;//23;
    1 y/ d4 }* g$ R: S
  17. float Ki=0.000201;
    " V( `" Q- `, Y; q! c( Y

  18. & @% u) S& \4 P
  19. float nPower;
    - r+ D" b. H/ y& |
  20. MePort lightsensor_6(6);
    / ~/ s% B1 M9 [4 O# b0 x
  21. MePort lightsensor_8(8);
    7 `( ^* U. ?: z& B
  22. MeDCMotor motor_9(9);8 W1 f$ a( ^! c! {2 i5 C
  23. MeDCMotor motor_10(10);
    ; z, Y- N* l8 }$ k0 {
  24. unsigned long previousMillis = 0;
    " S% d% w' y7 a+ R! o9 I5 I( S
  25. const long interval = 1;8 ?8 O1 a8 s8 ]
  26. " j  W( |4 f1 z& B4 B
  27. void setup(){
    " [/ d% q5 c+ {
  28.     lightsensor_6.dWrite1(1);- {: }) k( z3 K
  29.     nPower = 160;
    1 Z  b9 q3 k( E0 k9 C( H
  30.     Error=0;
    ' Q9 k  K5 `5 A. U# z
  31.     ErrorAcc=0;- W# }+ [) ?$ a$ h: B
  32. }
    5 J9 j2 w! q$ E" s# H9 I
  33. 6 _( v, p$ ~! c
  34. void loop(){
    0 V8 j' D/ s6 W
  35.   unsigned long StartTime = millis();& o8 N8 ]) P0 c1 F. }) E1 P
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . K; u0 @' \3 ]% X* v- M, w
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    " c+ c4 a  ?& i7 {
  38.     ErrorAcc +=  nError*Kd ;; M- M) i7 K; \( t, [1 V
  39.     ErrorDec -=  nError*Ki ;( g6 A) m: t% j) W
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 @2 _. ^% j$ p  Y
  41.     if(nError < 80 && nError > -80){+ l4 x/ @$ m. x6 K) U& }
  42.       if(nPower < MaxPower){8 h9 P1 `6 q; P3 M' L( n
  43.         nPower += 3;! Z: g: X' ^5 _9 F5 J. {9 S' A
  44.       }7 {/ r* s; f; s. N/ j4 C
  45.     } else{" K5 f6 l6 e* R0 O! u) t
  46.       if(nPower > MinPower){
    9 z2 |, ^0 C& F7 R& [
  47.         nPower -= 2;
    5 B3 u# b8 k' p, P7 j% l3 F! y+ h
  48.       }
    - C) r: p) I0 Z  i" M
  49.     } : M+ v1 A' m) {7 z8 T. j1 }
  50.     MotoL(nPower-Error);
    % r0 q( j. m* q- E! T$ [
  51.     MotoR(nPower+Error);   
    1 o1 _9 G% K4 T
  52.   }else{
    ) w" b' ^3 z$ W" G) o$ K3 S, A
  53.     motor_9.run(0);
    0 p! R! d1 {  {0 {3 N' V
  54.     motor_10.run(0);/ [$ @' F, L' |+ E; I- F/ g# V9 [
  55.   }! G/ _8 _9 K" a1 G0 }0 W& k: c: |
  56.   do{}while(millis() - StartTime < interval);
    ( g) _. h0 X7 q1 s% w& I
  57. }* t- N; |* H5 X' A0 K+ g% r0 c

  58. . V3 Q/ ^: D) H0 L1 T+ m& o
  59. void MotoL(int Power){+ x! u( p5 ^+ H  J
  60.   if (Power > MaxSpeed){
    5 l0 M  M! _; t. V4 a4 i
  61.      Power = MaxSpeed;7 u4 P" e* V' ?5 n( @6 I; A( g
  62.   } 3 w" G; Y( P7 o6 T$ t" x
  63.   if (Power < -MaxSpeed){
    9 C6 {0 d( g! L+ ~2 p
  64.      Power = -MaxSpeed;. k9 |2 m4 t' \" V) {( Z
  65.   }
    : h- \" J1 a7 S: b7 M$ H
  66.   motor_9.run(Power);! h' k' r  y; ]" k. t+ {. F+ l
  67. }  0 U9 m+ @: c3 U) ^  r( W# Q# `

  68. 7 `+ Y, ?. z* T" d
  69. void MotoR(int Power){# M' \1 J4 m: D
  70.   if (Power > MaxSpeed){: F+ `0 C7 z! n( m8 i. p
  71.      Power = MaxSpeed;* {+ c7 C2 R0 e, @- P3 @+ m0 s
  72.   } : H$ ~; N* L; {$ J  c' f
  73.   if (Power < -MaxSpeed){; P: \* g2 ~/ E) Z) V0 Q- ^
  74.      Power = -MaxSpeed;/ A: M* Y& t! z
  75.   } 6 Y9 t$ t, h& e+ t. z
  76.   motor_10.run(Power);
    2 S( q% H% W! ], U9 r; y2 ]
  77. }  
複製代碼

0 P) ?7 \5 [' g& R
2 g8 x6 y; r/ O6 y3 r
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
! ?" h' w* Q* A! `" _% N
' r) ?- ^" b; y您好,不知是否能向您請教。  g8 c6 ^' V/ P8 K* T/ @0 `" B
目前和宋修賢老師在處理Ardui Car& j: `9 n0 l3 y6 u: g
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
5 E6 A) y/ }) f: i- |, |  A1 Q1 s% ^4 o: _* a! u6 D  l: @; ~+ z! O: H2 y
但基於想追求更精簡的程式所以還是想請問一下
: Y+ c$ M$ [; F% Q5 t5 X! `/ u# G就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
7 z7 L, e- D! L) w不知道您是否願意教我可以如何處理
  M" w, Z! y) G# r1 A2 N+ C  T  Q8 {: e; j3 U/ p$ c1 R2 P8 p
( A9 p$ f0 P& w: n, J+ a1 A2 x- p
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;" u0 i9 s7 H. R; `
  2. double CNY70Val = 1000;
    ' J3 E$ ~; s! X: b# s  u# z& k% {  o
  3. int interror = 0;
    - a6 x0 n5 b9 `( g
  4. int olderror = 0;
    * A, d8 }+ M" l- L" U
  5. double values;8 w. {% a  R; M

  6. ; ^/ z) i" S% Y! h1 Y4 V# U

  7. % ]# ^% F$ O( o3 N: P
  8. void CNY70(). f1 o, z7 x/ V; t) M+ g& I
  9. {
    + t- Z: w& W& f
  10.   valuesRR = analogRead(RR)
    + V" U; m3 e7 ^& V" u, W
  11.   valuesMR = analogRead(MR);
    1 g8 E" {1 U4 p) T2 }0 t9 o
  12.   valuesMM = analogRead(MM);
    ) M# u% {0 h3 F0 P( Q1 J/ F
  13.   valuesML = analogRead(ML);  M0 Y; B& Y. E" a. k
  14.   valuesLL = analogRead(LL);
    0 p5 i% X7 j3 E7 ]" d/ m
  15. # Q, S/ M7 h4 W+ {7 g: F
  16.   if (valuesRR > CNY70Val)& M! i+ y. [4 f( ?6 j( Q8 b
  17.     valuesRR = CNY70Val;4 t! n4 v/ i9 X8 Y5 U* m: q
  18.   if (valuesMR > CNY70Val)' h7 T$ ]0 W9 E) `6 X+ U
  19.     valuesMR = CNY70Val;! w" L' Z* o2 D$ n# \
  20.   if (valuesMM > CNY70Val)
    / m* B7 {3 p4 D: D
  21.     valuesMM = CNY70Val;
    6 V: [# Y0 l8 U
  22.   if (valuesML > CNY70Val)
    # V- Y  d1 G8 Z$ X/ ^. E( M4 n
  23.     valuesML = CNY70Val;/ C5 }( I( s' Y  h$ ~( T
  24.   if (valuesLL > CNY70Val)" R: g5 u! f8 N. R1 G6 S9 {
  25.     valuesLL = CNY70Val;$ a# d0 d0 q+ h0 H
  26.   I9 F, E" ^; ^
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 }, U2 @+ z; |$ l& E# [
  28. }1 A7 _5 E8 \: b5 z7 c3 i

  29. + j# z1 s; E: I1 H
  30. void Car()
    0 x+ b' F- i: _2 Z. {5 g3 ?" X9 z2 |
  31. {7 I. i  n( `* r0 R8 E: O& K
  32.   while (1) {( ~* d0 B+ Y( a7 N
  33.     CNY70();
    6 F' J* k6 m0 Y* m

  34. & C7 ~3 g# Q' t4 i+ T+ w5 f
  35.     int error = ((int)values);
    " T) F* l( H" i3 w& j- N) C6 y. N
  36.     interror += error;4 V/ o/ Q( T% G. B0 `0 W
  37.     int lasterror = error - olderror;
    ' J5 {5 B6 h7 f9 P$ b# [5 |0 }% C
  38.     olderror = error;* U4 o+ {; e# z) i6 d
  39.     int power = error / 5 + interror / 10000 + lasterror;% W, O# Q7 ]# c' s. b5 n$ N8 t/ n

  40. ! d! g' o$ ~. R# ~
  41.     if (power > MotoSpeed)
    ' J: \- r3 I8 G! _+ D0 C$ l8 R
  42.       power = MotoSpeed;
    & G8 M/ k0 @, U( x- z  _
  43.     if (power < -MotoSpeed)
    ' u: P/ a" v/ i( n- e4 \
  44.       power = -MotoSpeed;6 X: d8 I1 Q% ^* P1 m
  45. $ _! s- r& k. _1 l
  46.     if (power > 0)
    * o% n. p4 m% M& |
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。/ Q3 q- }' }( J3 h! V
  48.     else' {5 L* j3 d& \
  49.       Speed(MotoSpeed + power, MotoSpeed);! Q& N! U; v1 _; L' K& R2 O% f& P
  50.   }# v! P) a1 U# W' j" }
  51. }
複製代碼
7 u3 p2 X' }! T$ n+ C

$ R  f& j$ y2 p' P
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-7-11 08:34 , Processed in 0.027108 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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