Boe-Bot Car PID控制馬達的問題

jason83530

各位大大:
我是今年才踏進這個領域

在網路上看了很多有關PID控制的東西

我還是不知道怎麼把它應用到BBCar裡面

我的問題是:

1.PID可以幫我的BBCar什麼? 是車子跑起來的順暢度還是?

2.那我程式該怎麼打? 能給一些範例嗎?

例如

我讓車子轉彎:

打

DO

PLUSOUT 13, 650
PLUSOUT 12, 650

LOOP

這樣

那PID加進去有什麼好處?
怎麼加進去?
這是我想問的重點

我看官網的論壇那個程式碼

我也不知道能做什麼

如下:

' PidMathExample.bs2
' Demonstrates how a combination of proportional, integral, and
' derivative control influence error correction in a feedback loop.

' {$STAMP BS2}
' {$PBASIC 2.5}

SetPoint       CON     0                     ' Set point
Kp             CON     10                    ' Proportionality constant
Ki             CON     10                    ' Integral constant
Kd             CON     10                    ' Derivative constant

Current        CON     0                     ' Array index - current error
Accumulator    CON     1                     ' Array index - accumulated error
Previous       CON     2                     ' Array index - previous error
Delta          CON     3                     ' Array index - change in error

sensorInput    VAR     Word                  ' Sensor input variable
error          VAR     Word(4)               ' Four different types of errors
p              VAR     Word                  ' Proportional term
i              VAR     Word                  ' Integral term
d              VAR     Word                  ' Derivative term
drive          VAR     Word                  ' Output

DO

  DEBUG "Enter sensor input value: "
  DEBUGIN SDEC sensorInput

  ' Calculate error.
  error(Current) = SetPoint - sensorInput

  ' Calculate proportional term.
  p = Kp * error(current)

  ' Calculate integral term.
  error(Accumulator) = error(Accumulator) + error(Current)
  i = Ki * error(Accumulator)

  ' Calculate derivative term.
  error(Delta) = error(Current) - error(Previous)
  d = Kd * error(delta)

  ' Calculate output.
  drive = p + i + d

  ' Display values.
  DEBUG CR, CR, "ERROR", CR,
        SDEC ? SetPoint, SDEC ? sensorInput, SDEC ? error(Current), CR,
        "PROPORTIONAL", CR,
        SDEC ? Kp, SDEC ? error(Current), SDEC ? p, CR,
        "INTEGRAL", CR,
        SDEC ? Ki, SDEC ? error(accumulator), SDEC ? i, CR,
        "DERIVATIVE", CR,
        SDEC ? Kd, SDEC ? error(Delta), SDEC ? d, CR,
        "OUTPUT", CR,
        SDEC ? p, SDEC ? i, SDEC ? d, SDEC ? drive, CR, CR

  ' Save current error to previous error before next iteration.
  error(Previous) = error(Current)

LOOP

請各位大大指點迷津  感謝!

jason83530

回復 2# g921002

大大你好

我想控制BBCar的馬達

星期六有個比賽

算是走迷宮吧

我想知道PID在這台自走車上

能發揮什麼功用?

還有

我該如何打程式?

jason83530

回復 4# stanley21

感謝
那  i 和 d的部分有沒有人能說明一下?

不是原理

是

例如

' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - FollowingBoeBot.bs2
' Boe-Bot adjusts its position to keep objects it detects in zone 2.

' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

DEBUG "Program Running!"

' -----[ Constants ]----------------------------------------------------------
Kpl            CON     -35
Kpr            CON     35
SetPoint       CON     2
CenterPulse    CON     750
' -----[ Variables ]----------------------------------------------------------

freqSelect     VAR     Nib
irFrequency    VAR     Word
irDetectLeft   VAR     Bit
irDetectRight  VAR     Bit
distanceLeft   VAR     Nib
distanceRight  VAR     Nib
pulseLeft      VAR     Word
pulseRight     VAR     Word

' -----[ Initialization ]-----------------------------------------------------

FREQOUT 4, 2000, 3000

' -----[ Main Routine ]-------------------------------------------------------

DO

  GOSUB Get_Ir_Distances

  ' Calculate proportional output.

  pulseLeft =  SetPoint - distanceLeft  * Kpl + CenterPulse
  pulseRight = SetPoint - distanceRight * Kpr + CenterPulse

  GOSUB Send_Pulse

LOOP
Get_Ir_Distances:
  distanceLeft = 0
  distanceRight = 0
  FOR freqSelect = 0 TO 4
    LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency
FREQOUT 8,1,irFrequency
    irDetectLeft = IN9
    distanceLeft = distanceLeft + irDetectLeft

    FREQOUT 2,1,irFrequency
    irDetectRight = IN0
    distanceRight = distanceRight + irDetectRight
  NEXT
  RETURN
' -----[ Subroutine – Get Pulse ]---------------------------------------------

Send_Pulse:
  PULSOUT 13,pulseLeft
  PULSOUT 12,pulseRight
  PAUSE 5
  RETURN
.................我是分隔線..................................................................................


所以p就是乘一個比例再給馬達速度 是這樣嗎
(pulseLeft =  SetPoint - distanceLeft  * Kpl + CenterPulse
  pulseRight = SetPoint - distanceRight * Kpr + CenterPulse)

.......................................分隔線.....................
問題:


1.那i 和d呢?程式的部分我怎麼試都沒有i和d的感覺?

2.如果要讓他記憶迷宮之類的路線 pid可以幫到他什麼忙
是不是記憶完後要靠IR來微調的這方面呢?

jason83530

不好意思

如果沒辦法在短時間內給我清楚的說明

是否能給個方向

因為時間有點急迫

下星期要交報告了...

jason83530

已實際操作過了

也試過修改某些數值然後看結果

基本上瞭解了 但不知道算不算徹底
................................................

SetPoint為0到4
設定2的話

如果IR(Left)給3,IR(Right)給0
2-3=-1                   2-0=2               
1*-35=-35             2*35=70
750-35=715         750+70=820
所以左右馬達會以這兩個數值下去轉

大致上是這樣吧
有遺漏嗎