Transcript 競速自走車

競速自走車
主講者
張 育 誠
競速自走車影片
循跡自走車
Line Tracer
Singapore Robo Grand Prix 2009
2009年第五屆人工智慧單晶片競速自走車決賽
2009 日本RoboTracer
2009 日本自走車比賽
競速場地說明
自走車設計
認識InnoRacer
InnoRacer 簡介:
1.BC2 核心
2.USB程式下載
3.BASIC 語言及函數指令控制
4.提供數位及類比PID控制
5.具馬達轉速回饋及過載保護功能
6.跑道記憶功能
7.內建G-Sensor, 提供彎道曲率計算
8.額外cmdbus模組擴充功能
9.適合教育學習及比賽用車
BC2核心
反射式紅外
線感測器
萬向滾珠輪
感測器校正
按鈕
系統重置按
鈕
Buzzer發聲元
件
Cmdbus模組
外接接頭
按鈕及燈號
操作
馬達速度控
制晶片M1
跑道記憶控
制晶片P1
馬達驅動晶
片
遙控模型競
技用海綿胎
加速度感測器
16000rpm 附
編碼器驅動
馬達
6V 2200mah
NiMh電池
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
按鈕及LED燈號操作
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
USB
Btn1
Led2
Btn2
Led3
Btn3
Led4
Btn4
P23
P22
P21
P20
P19
P18
P17
P16
…..
Led1
BC2
…..
實驗一: LED控制
‘===========說明: 依序點亮與熄滅四個LED=========================
SUB Main()
Dim bLED As Byte ' 儲存要設定的LED腳位
'-----------------------------------------------------------------------'
無窮迴圈，反覆亮滅LED
'-----------------------------------------------------------------------DO
'-----------------------------------------------------------------------'
FOR迴圈，依序設定控制腳位20~23的LED，讓各LED點亮0.5秒
'-----------------------------------------------------------------------For bLED=20 To 23
HIGH bLED
' 點亮bLED腳位設定的LED
PAUSE 500
' 暫停0.5秒
LOW bLED
' 熄滅bLED腳位設定的LED
PAUSE 500
' 暫停0.5秒
Next
LOOP
End SUB
實驗二: 按鈕與LED
‘==說明: 依序偵測四個按鈕，執行點亮LED的動作=====
#DEFINE BUTTON_1 16
#DEFINE BUTTON_2 17
#DEFINE BUTTON_3 18
#DEFINE BUTTON_4 19
Sub Main()
Dim bCnt1 As Byte = 0
Dim bCnt2 As Byte = 0
Dim bCnt3 As Byte = 0
Dim bCnt4 As Byte = 0
BEGIN:
Pause 10 ' 等待 10 ms
‘依序偵測四個按鈕是否有被按下
‘並在偵測到按下後跳到對應的標籤位置
Button BUTTON_1, 0, 255, 20, bCnt1, 1, TURN_ON_LED1
Button BUTTON_2, 0, 255, 20, bCnt2, 1, TURN_ON_LED2
Button BUTTON_3, 0, 255, 20, bCnt3, 1, TURN_ON_LED3
Button BUTTON_4, 0, 255, 20, bCnt4, 1, TURN_ON_LED4
Goto BEGIN ' 回到開始位置
‘執行點亮LED的副程序
TURN_ON_LED1:
TurnOnLED(20)
Goto BEGIN ' 回到開始位置
TURN_ON_LED2:
TurnOnLED(21)
Goto BEGIN ' 回到開始位置
TURN_ON_LED3:
TurnOnLED(22)
Goto BEGIN ' 回到開始位置
TURN_ON_LED4:
TurnOnLED(23)
Goto BEGIN ' 回到開始位置
End Sub
Sub TurnOnLED(bLED As Byte)
High bLED
Pause 500
Low bLED
Pause 500
End Sub
' 點亮bLED腳位設定的LED
‘ 暫停0.5秒
' 熄滅bLED腳位設定的LED
' 暫停0.5秒
如何控制馬達
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
BC2核心
馬達速度控
制晶片M1
馬達驅動晶
片
16000rpm 附
編碼器驅動
馬達
實驗三: 馬達控制
‘說明: 根據輸入值操作馬達動作，並設定停止方式
‘注意: 馬達轉動會讓自走車移動，請先墊高自走車讓輪胎離地，再開始測試
Peripheral myM As RacerM1 @ 3
' 設定馬達控制模組參數名稱
Sub Main()
Dim bKey As Byte
‘ 儲存輸入值
Dim iVelL, iVelR As Integer
' 左右輪控制參數
‘無窮迴圈，可以反覆輸入馬達控制參數
Do
Debug CLS
‘清除終端視窗顯示文字
Debugin "請輸入左輪參數(-1024~1024): ", iVelL
Debug iVelL, CR
Debugin "請輸入右輪參數(-1024~1024): ", iVelR
Debug iVelR, CR
myM.SetVelAB(iVelL, iVelR)
' 設定左右輪轉速與方向
Debugin "請決定停止方式(0: Stop, 1: Brake): ", bKey
Debug bKey, CR
If bKey=0 Then
myM.StopDual()
‘ 以Stop方式停止兩輪轉動
Else
myM.BrakeDual()
‘ 以Brake方式停止兩輪轉動
End If
Keyin "按下任意鍵繼續", %CHR bKey
LOOP
End Sub
IR紅外線感測器
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
調整感度
逆時針旋轉感度增加
順時針旋轉感度減少
IR感測器背面位置
有反射訊號時會亮紅
燈無反射則不亮
IR感測電壓感度調整晶片
萬向滾珠輪
IR感測器正面
IR感測器接收訊號端
IR感測器發射訊號端
最近辨識距離為d=2mm
d
最大辨識距離約為d=20mm (需調整可變電阻)
離地距離越近則辨識解析度越高
離地距離越遠則辨識解析度越低
特性曲線及電路
CH
IR0
IR1
IR2
IR3
IR4
…..
P11
P7
P6
P5
P4
P3
P2
P1
P0
USB
IR5
IR6
ST
…..
BC2
實驗前需利用可變電阻將所有感測器感度都調整成相同比例
實驗四: 紅外線感測
‘=========說明: 反覆偵測紅外線感測值，並顯示在終端視窗=======
Sub Main()
Dim bIR,ST,CH As Byte
' 儲存紅外線感測值
Debug CLS
' 清除終端視窗文字
Debug "紅外線感測值: “,CR ' 輸出文字訊息到終端視窗
Debug “
ST感測值: “,CR ' 輸出文字訊息到終端視窗
Debug “
CH感測值: " ' 輸出文字訊息到終端視窗
‘無窮迴圈，反覆偵測紅外線感測值
Do
bIR = Readport(0) ' 讀取紅外線感測器回傳值
ST=in(11)
CH=in(7)
Debug CSRXY(15, 1), %BIN bIR
Debug CSRXY(15, 2), %BIN ST
Debug CSRXY(15, 3), %BIN CH
' 以二進制顯示感測值於終端視窗
Loop
End Sub
紅外線尋跡控制
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
實驗五: 紅外線尋跡前進(3個感測器)
6
5
4
3
2
1
0
實驗五: 紅外線尋跡前進(3個感測器)
‘說明: 利用三組紅外線感測值，沿軌道行走。
Peripheral myM As RacerM1 @ 3 ' 設定馬達控制模組參數名稱
#DEFINE NORMAL_SPEED_R 170 ' 設定右輪直線速度值
#DEFINE NORMAL_SPEED_L 170 ' 設定左輪直線速度值
' 依序設定不同感測值的修改速度值
#DEFINE ERROR_1 80
#DEFINE ERROR_2 50
#DEFINE ERROR_3 30
#DEFINE ERROR_4
0
#DEFINE ERROR_5 -30
#DEFINE ERROR_6 -50
#DEFINE ERROR_7 -80
Sub Main()
Dim IR2, IR3, IR4, Sensor As Byte
‘存感測值
Dim R, L, Err As Integer
‘ 儲存左右輪轉速與誤差值
Pause 2000 ‘等待兩秒
‘無窮迴圈，反覆偵測軌道感測值，修正馬達轉向與速度
Do
IR2 = In(2) ‘ 取得第2腳位的感測值
IR3 = In(3) ‘ 取得第3腳位的感測值
IR4 = In(4) ‘ 取得第4腳位的感測值
Sensor = (100 * IR4) + (10 * IR3) + IR2
‘ 將偵測到的值拼為一個判斷值
‘根據感測器回傳值，判斷軌道位置並設定修改速度值
Select Sensor
Case 011
Err = ERROR_2
Case 001
Err = ERROR_3
Case 101
Err = ERROR_4
Case 100
Err = ERROR_5
Case 110
Err = ERROR_6
Case 111
If Err<0 Then
Err = ERROR_7
Elseif Err>0 Then
Err = ERROR_1
End If
Case 000
Err = ERROR_4
End Select
‘根據修改速度值，計算左右輪的轉速值
R = NORMAL_SPEED_R + Err
L = NORMAL_SPEED_L - Err
‘判斷是否超過模組極限值, 如果超過則設定為極限值
If R>1024 Then
R = 1024
Elseif R<-1024 Then
R = -1024
End If
If L>1024 Then
L = 1024
Elseif L<-1024 Then
L = -1024
End If
myM.SetVelAB(L, R) ‘根據計算結果設定模組速度值
Loop
End Sub
實驗五: 紅外線尋跡前進(7個感測器)
6
5
4
3
2
1
0
實驗六: 紅外線尋跡前進(7個感測器)
'說明: 利用七組紅外線感測值，沿軌道行走。
Peripheral myM As RacerM1 @ 3 '設定馬達控制模組參數名稱
#DEFINE NORMAL_SPEED_R 210 '設定右輪直線速度值
#DEFINE NORMAL_SPEED_L 210 '設定左輪直線速度值
'依序設定不同感測值的修改速度值
#DEFINE ERROR_1
230
#DEFINE ERROR_2
200
#DEFINE ERROR_3
120
#DEFINE ERROR_4
90
#DEFINE ERROR_5
60
#DEFINE ERROR_6
30
#DEFINE ERROR_7
0
#DEFINE ERROR_8
-30
#DEFINE ERROR_9
-60
#DEFINE ERROR_10
-90
#DEFINE ERROR_11
-120
#DEFINE ERROR_12
-200
#DEFINE ERROR_13
-230
Sub Stop() ‘停止副程式
myM.BrakeDual()
End Sub
Sub Main()
Dim Sensor As Byte
'儲存感測值
Dim R, L, Err As Integer '儲存左右輪轉速與修改速度值
Pause 1000
Do '無窮迴圈，反覆偵測軌道感測值，修正馬達轉向與速度
Sensor = Readport(0)
‘取得埠0的感測值
Sensor = Sensor And &H7F
‘根據感測器回傳值，判斷軌道位置並設定修改速度值
Select Case Sensor
Case &B0111111
Err = ERROR_1
Case &B0011111
Err = ERROR_2
Case &B1011111
Err = ERROR_3
Case &B1001111
Err = ERROR_4
Case &B1101111
Err = ERROR_5
Case &B1100111
Err = ERROR_6
Case &B1110111
Err = ERROR_7
Case &B1110011
Err = ERROR_8
Case &B1111011
Err = ERROR_9
Case &B1111001
Err = ERROR_10
Case &B1111101
Err = ERROR_11
Case &B1111100
Err = ERROR_12
Case &B1111110
Err = ERROR_13
Case &B1111111
Stop()
Goto FINISH '跳到結束標籤的位置
End Select
R = NORMAL_SPEED_R + Err ‘計算左右輪的轉速值
L = NORMAL_SPEED_L - Err
If R>1024 Then '判斷設定值是否超過模組極限值, 則設定為極限值
R = 1024
Elseif R<-1024 Then
R = -1024
End If
If L>1024 Then
L = 1024
Elseif L<-1024 Then
L = -1024
End If
myM.SetVelAB(L, R) ‘根據計算結果設定模組速度
Loop
FINISH:
End Sub
自走車控制理論介紹(PD控制)
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
軌跡預測1
Error=0
6
5
4
3
2
1
0
Error=100
軌跡預測2
6
5
4
3
2
1
0
Error=50
軌跡預測3
6
5
4
3
2
1
0
軌跡預測4
Error=50
軌跡預測5
Error=50
數位回授控制
比例微分控制法則(PD):
根據誤差值(Error)來調整左右輪的速度差
若是軌跡在中心軸左邊，誤差為負值，比例
控制根據誤差量調整右輪速度大於左輪速度；
反之亦然
若誤差持續擴大，微分控制可以加大速差
數位回授控制
Sd(n) = Kp*e(n) + Kd[e(n)-e(n-1)]
S(n)=S_normal-Sd(n)
Kp
速度控制
指令
+
+
Motor
-
+
微分
自走車在
軌跡上的
位置
Kd
IR sensor
回授
計算Error值
數位回授控制
誤差值(Error)與左右輪速度變化值
Speed
右輪速度命令
左輪速度命令
由回授控制決定變化曲線
馬達反轉點
0
Error
實驗七: PD控制
‘說明: 使用PID控制自走車沿軌道行走。
Peripheral myM As RacerM1 @ 3 ' 設定馬達控制模組參數名稱
#DEFINE KP
1 ' 設定PID的參數值
#DEFINE KI
0
#DEFINE KD
10
#DEFINE SCALE
0
#DEFINE NORMAL_SPEED_R
250 ' 設定右輪直線速度值
#DEFINE NORMAL_SPEED_L
250 ' 設定左輪直線速度值
#DEFINE ERROR_1
360 ' 設定不同感測結果的誤差設訂值
#DEFINE ERROR_2
280
#DEFINE ERROR_3
180
#DEFINE ERROR_4
130
#DEFINE ERROR_5
80
#DEFINE ERROR_6
40
#DEFINE ERROR_7
0
#DEFINE ERROR_8
-40
#DEFINE ERROR_9
-80
#DEFINE ERROR_10
-130
#DEFINE ERROR_11
-180
#DEFINE ERROR_12
-280
#DEFINE ERROR_13
-360
Sub Stop() ‘停止副程式
myM.BrakeDual()
End Sub
Sub Main()
Dim Sensor As Byte
‘ 儲存感測值
Dim R, L As Integer
‘ 儲存左右輪轉速與誤差值
Dim Integral As Integer
‘ 儲存誤差積分值
Dim Derivative As Integer
‘ 儲存誤差微分值
Dim Err, PreErr As Integer
‘ 儲存前一次的誤差值
Dim Out As Integer
‘ 儲存PID計算結果
Dim Control As Integer
‘ 儲存PID控制值
Out
=0
Integral
=0
PreErr
=0
Pause 1000
DO ‘無窮迴圈，反覆偵測黑線感測值，修正馬達轉向與速度
Sensor = Readport(0) And &B01111111
' 取得埠0的感測值
Select Case Sensor ‘根據感測器回傳來判斷軌道位置並設定誤差
Case &B0111111
Err = ERROR_1
Case &B0011111
Err = ERROR_2
Case &B1011111
Err = ERROR_3
Case &B1001111
Err = ERROR_4
Case &B1101111
Err = ERROR_5
Case &B1100111
Err = ERROR_6
Case &B1110111
Err = ERROR_7
Case &B1110011
Err = ERROR_8
Case &B1111011
Err = ERROR_9
Case &B1111001
Err = ERROR_10
Case &B1111101
Err = ERROR_11
Case &B1111100
Err = ERROR_12
Case &B1111110
Err = ERROR_13
Case &B1111111
Stop()
Goto FINISH
End Select
Integral
= Integral + Err ‘PID計算公式
Derivative = Err - PreErr
Out
= (KP * Err) + (KI * Integral) + (KD * Derivative)
PreErr
= Err
Control
= Out >> SCALE
R = NORMAL_SPEED_R + Control ‘根據誤差值計算左右輪的轉速值
L = NORMAL_SPEED_L - Control
If R>1024 Then
‘判斷設定值是否超過模組極限值, 則設定為極限值
R = 1024
Elseif R<-1024 Then
R = -1024
End If
If L>1024 Then
L = 1024
Elseif L<-1024 Then
L = -1024
End If
myM.SetVelAB(L, R) ‘根據計算結果設定模組速度值
Loop
FINISH:
End Sub
M1內建PD控制
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
M1內建感測器校正功能
利用M1內建PD控制時
需要先完成M1內建之IR紅外線感測器感度校正
校正程序方法為:
1.按下CAL_BTN按鈕一直到紅色CAL_LED亮起 (約按住5秒)
2.此時將InnoRacer平放在競速場地中 並對齊跑道白線
3.並以馬達軸線與白線的交插點為圓心將車子旋轉
4.以一定速度緩慢的將所有IR紅外線感測器來回劃過白線部份
5.完成以上動作之後再按下CAL_BTN按鈕以結束校正
實驗八: M1內建PD控制
'說明: 使用RacerM1模組，使用內建PID控制，偵測跑道行駛。
Sub Main()
Peripheral myM1 As RacerM1 @ 3 ' 設定馬達控制模組參數名稱
Dim bIr AS BYTE
#DEFINE PID_P
4 'M1模組內部PID參數值 (0 ~ 255)
Debug CLS ‘ 清除終端視窗文字
#DEFINE PID_I
0
InitM1()
‘ 執行設定模組副程序
#DEFINE PID_D
40
Do '反覆偵測紅外線感測值，值到判斷軌道在中間位置
#DEFINE PID_SCALE
0
myM1.GetIR(bIr)
'取得紅外線感測值
#DEFINE IR_POWER 70
’設定紅外線感測強度
Loop Until (bIr And &B1000) '判斷軌道是否在中間
‘模組內部最高最低速限，以及直線速度 (-1024 ~ 1024)
Pause 3000
#DEFINE
MAX_SPEED_L
1024
myM1.SpdCtrlOn(0)
‘ 啟動PID速度控制
#DEFINE
MAX_SPEED_R
1024
Do '無窮迴圈
#DEFINE
CENTER_SPEED_L
220
Loop
#DEFINE
CENTER_SPEED_R
220
End
Sub
#DEFINE
MIN_SPEED_L
-1024
#DEFINE
MIN_SPEED_R
-1024
'模組各別感測階段的誤差值 (0 ~ 127)
#DEFINE ERROR_1
10
#DEFINE ERROR_2
20
#DEFINE ERROR_3
32
#DEFINE ERROR_4
45
#DEFINE ERROR_5
70
#DEFINE ERROR_6
90
Sub InitM1() '設定M1模組各項預設值
‘設定M1模組內部PID參數值
myM1.SetP(PID_P)
myM1.SetI(PID_I)
myM1.SetD(PID_D)
myM1.SetScalar(PID_SCALE)
myM1.SetIRThreshold(IR_POWER)
‘設定M1模組內部最高最低速限，以及直線速度
myM1.SetSpdCtrlA(MIN_SPEED_L, MAX_SPEED_L)
myM1.SetSpdCtrlB(MIN_SPEED_R, MAX_SPEED_R)
myM1.SetStraight(CENTER_SPEED_L, CENTER_SPEED_R)
‘設定M1模組各別感測階段的誤差值
myM1.SetErrScale(ERROR_1, ERROR_2, ERROR_3, ERROR_4, ERROR_5, ERROR_6)
End Sub
進階IR紅外線感測器類比控制
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
類比感測器數值正規化
調整前值域
調整後值域
IR0
=0~350
IR0
=0~400
IR1
=20~480
IR1
=0~400
IR2
=50~330
IR2
=0~400
IR3
=7~457
IR3
=0~400
IR4
=0~545
IR4
=0~400
IR5
=100~350
IR5
=0~400
IR6
=14~407
IR6
=0~400
類比感測器數值正規化
Y輸出值  Y最小值
Y最大值  Y最小值

X輸入值  X最小值 X最大值  X最小值
 Y最大值  Y最小值 
Y輸出值  Y最小值  
 *  X輸入值  X最小值 
 X最大值  X最小值 
每顆感測器的類比輸出特性會因實際狀況而有所差異
因此需透過演算法將感測器輸出調整至相同的值域範圍
二次曲線預測
跑道中心
跑道線
數位
類比
y  ax2  bx  c
回饋訊號
大小(V)
感測器與跑道線相對位置(V)
1. 透過IR感測器類比式回授可更精準得知感測器相對於跑道現的偏移量
2. 類比回授訊號曲線可近似於一二次曲線 y  ax  bx  c
2
二次曲線預測
y  ax2  bx  c
跑道中心
1. 取最大三個感測器回授值
代入聯立合併計算，可求
出a,b,c之數值
2. 透過微分則可得知二次曲
線頂點位置 x   b 2a
3. 如此可得知跑道中心位置
跑道線
回饋訊號
大小(V)
e  3 x
4. 相較於數位預測更為精準
感測器與跑道線相對位置(V)
0
1
2
3
4
5
6
實驗九: M1內建PD控制(類比)
'說明: 使用RacerM1模組，使用內建PID控制，偵測跑道行駛。
Sub Main()
Peripheral myM1 As RacerM1 @ 3 ' 設定馬達控制模組參數名稱
Dim bIr AS BYTE
#DEFINE PID_P 4 'M1模組內部PID參數值 (0 ~ 255)
Debug CLS ‘ 清除終端視窗文字
#DEFINE PID_I 0
InitM1()
‘ 執行設定模組副程序
#DEFINE PID_D 40
Do
'反覆偵測紅外線感測值，值到判斷軌道在中間位置
#DEFINE PID_SCALE 0
myM1.GetIR(bIr)
'取得紅外線感測值
#DEFINE IR_MODE 1
‘設定紅外線感測模式
Loop Until (bIR And &B1000) '判斷軌道是否在中間
#DEFINE IR_POWER 70
’設定紅外線感測強度
Pause 3000
‘模組內部最高最低速限，以及直線速度 (-1024 ~ 1024)
myM1.SpdCtrlOn(0)
‘ 啟動PID速度控制
#DEFINE
MAX_SPEED_L
1024
Do '無窮迴圈
#DEFINE
MAX_SPEED_R
1024
Loop
#DEFINE
CENTER_SPEED_L
220
End
Sub
#DEFINE
CENTER_SPEED_R
220
#DEFINE
MIN_SPEED_L
-1024
#DEFINE
MIN_SPEED_R
-1024
#DEFINE ERROR_1 10 ‘模組各別感測階段的誤差值 (0 ~ 127)
#DEFINE ERROR_2 20
#DEFINE ERROR_3 32
#DEFINE ERROR_4 45
#DEFINE ERROR_5 70
#DEFINE ERROR_6 90
Sub InitM1() '設定M1模組各項預設值
myM1.SetP(PID_P) ‘設定M1模組內部PID參數值
myM1.SetI(PID_I)
myM1.SetD(PID_D)
myM1.SetScalar(PID_SCALE)
myM1.SetIRMode(IR_MODE) ‘類比數位選擇
myM1.SetIRThreshold(IR_POWER)
‘設定M1模組內部最高最低速限，以及直線速度
myM1.SetSpdCtrlA(MIN_SPEED_L, MAX_SPEED_L)
myM1.SetSpdCtrlB(MIN_SPEED_R, MAX_SPEED_R)
myM1.SetStraight(CENTER_SPEED_L, CENTER_SPEED_R)
‘設定M1模組各別感測階段的誤差值
myM1.SetErrScale(ERROR_1, ERROR_2, ERROR_3, ERROR_4, ERROR_5, ERROR_6)
End Sub
加速度感測器
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
BC2核心
加速度感測
器
跑道記憶控
制晶片P1
1. 輪胎抓地力決定過彎向心加速度極限值
2. 每台自走車都會有在過彎時的向心加速度極限值
3. 利用過彎時產生的向心加速度值來決定要用多少速度過彎
實驗十: 加速度感測器
‘說明: 使用RacerP1模組，反覆偵測加速度感測值，並顯示在終端視窗。
Peripheral myP1 As RacerP1 @ 4 ' 設定多功能感測模組參數名稱
Sub Main()
Dim iX, iY As Integer
Debug CLS
Debug CSRXY(1,1), "加速度感測值"
‘無窮迴圈，反覆偵測紅外線感測值
Do
myP1.GetG(iX, iY)
‘ 取得加速度感測值
Debug CSRXY(1,2), “X: ”, CSRXY(4, 2), %DEC5R iX
Debug CSRXY(1,3), “Y: ”, CSRXY(4, 3), %DEC5R iY
Loop
End Sub
‘ 顯示X軸向加速度
‘ 顯示Y軸向加速度
跑道資料紀錄控制
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
實驗十一: 繞圈紀錄資料
‘說明: 使用RacerM1與RacerP1模組，使用內建PID控制，
SUB InitP1()
'設定P1模組各項預設值
‘偵測跑道行駛，並在行駛中紀錄跑道資訊
myP1.SetCrossTime(CROSS_TIME)
Peripheral myM1 As RacerM1 @ 3 '設定馬達控制模組參數名稱
'設定經過交叉軌道判斷時間
Peripheral myP1 As RacerP1 @ 4 '設定多功能感測模組參數名稱
myP1.AutoBeep(1)
'設定經過曲率變化點發出提醒
#DEFINE BRAKE_TIME
300 '設定停止時間
END SUB
#DEFINE CROSS_TIME
20
'設定交叉軌道判斷時間
Sub Main() '主程式
#DEFINE PID_P
17 'M1模組內部PID參數值 (0 ~ 255)
Dim Status As Byte
#DEFINE PID_I
0
Dim bIR As Byte
Debug CLS
'清除終端視窗文字
#DEFINE PID_D
115
InitM1()
'執行設定模組副程序
#DEFINE PID_SCALE
0
'模組內部最高最低速限，以及直線速度 (-1024 ~ 1024)
InitP1()
Do '反覆偵測紅外線感測值，值到判斷軌道在中間位置
#DEFINE
MAX_SPEED_L
1024
myM1.GetIR(bIr)
'取得紅外線感測值
#DEFINE
MAX_SPEED_R
1024
Loop Until (bIR And &B1000) '判斷軌道是否在中間
#DEFINE
CENTER_SPEED_L
220
myP1.StartRec(1)
'啟動紀錄
#DEFINE
CENTER_SPEED_R
220
Do 'Do迴圈，判斷是否進入紀錄狀態
#DEFINE
MIN_SPEED_L
-1024
#DEFINE
MIN_SPEED_R
-1024
myP1.GetRecStatus(Status)
'模組各別感測階段的誤差值 (0 ~ 127)
Loop Until Status=1
myM1.SpdCtrlOn(0)
'啟動PID速度控制
#DEFINE ERROR_1
1
Do 'Do迴圈，判斷是否偵測到開始點
#DEFINE ERROR_2
3
#DEFINE ERROR_3
7
myP1.GetRecStatus(Status)
#DEFINE ERROR_4
15
Loop Until Status=2
Do 'Do迴圈，判斷是否偵測到結束點
#DEFINE ERROR_5
25
#DEFINE ERROR_6
37
myP1.GetRecStatus(Status)
Sub InitM1() '設定M1模組各項預設值
Loop Until Status=0
myM1.SetP(PID_P) ‘設定M1模組內部PID參數值
myP1.StopRec()
‘結束紀錄
Pause BRAKE_TIME
‘等待停止時間
myM1.SetI(PID_I)
myM1.BrakeDual()
'停止左右輪
myM1.SetD(PID_D)
myM1.SetScalar(PID_SCALE)
End Sub
‘設定M1模組內部最高最低速限，以及直線速度
myM1.SetSpdCtrlA(MIN_SPEED_L, MAX_SPEED_L)
myM1.SetSpdCtrlB(MIN_SPEED_R, MAX_SPEED_R)
myM1.SetStraight(CENTER_SPEED_L, CENTER_SPEED_R)
'設定M1模組各別感測階段的誤差值
myM1.SetErrScale(ERROR_1, ERROR_2, ERROR_3, ERROR_4, ERROR_5, ERROR_6)
End Sub
實驗十二: 讀取紀錄值
'說明: 使用RacerP1模組，讀取儲存的紀錄資料。
Peripheral myP1 As RacerP1 @ 4 ' 設定多功能感測模組參數名稱
Sub Main() ‘主程式
Dim i As Byte
'路段讀取累加值
Dim bDir As Byte
'儲存曲率半徑方向值
Dim bSecCnt As Byte
'儲存路段數
Dim iGx, iGy As Integer
'儲存加速度感測值
Dim dwLenR, dwLenL As Dword ‘儲存路段距離值
Dim dwRad As Dword
'儲存曲率半徑
myP1.GetSecCnt(bSecCnt) '取得上次紀錄的總路段數
For i=0 To bSecCnt 'For迴圈依序取得各路段的資料
Debug "路段: ", %DEC3R i, CR
myP1.GetSecLen(i, dwLenR, dwLenL)
Debug "右輪距離: ", %DEC9R dwLenR, CR
Debug "左輪距離: ", %DEC9R dwLenL, CR
myP1.GetSecAvgG(i, iGx, iGy)
Debug "X軸向平均值: ", %DEC5R iGx, CR
Debug "Y軸向平均值: ", %DEC5R iGy, CR
myP1.GetSecMaxG(i, iGx, iGy)
Debug "X軸向最大值: ", %DEC5R iGx, CR
Debug "Y軸向最大值: ", %DEC5R iGy, CR
myP1.GetSecRadius(i, bDir, dwRad)
Debug "曲率半徑方向: ", bDir
Debug "曲率半徑: ", %DEC9R dwRad,CR,CR
Next
End Sub
直線加減速測試
實驗十三: 繞圈加減速控制
'說明: 使用RacerM1與RacerP1模組，在通過起始點後讀取總距離
‘並進行加速，超過設定距離再執行停止動作
SUB InitP1()
'設定P1模組各項預設值
myP1.SetCrossTime(CROSS_TIME) '設定經過交叉軌道判斷時間
END SUB
Sub Main()
DIM Status As Byte
DIM IR As Byte
DIM bCnt1 As Word
DIM LenR, LenL As Word
Debug CLS
'清除終端視窗文字
bCnt1 =0
InitM1()
'執行設定模組副程序
InitP1()
WAIT_BUTTON:
Do '判斷是否按下開始按鈕
Button(16, 0, 255, 255, bCnt1, 1, RACE_ROUND)
Loop
RACE_ROUND:
Do '反覆偵測紅外線感測值，值到判斷軌道在中間位置
myM1.GetIR(IR)
Loop Until (IR And &B1000)
Pause 2000
myP1.StartRec(0)
'開始紀錄資訊
Do 'Do迴圈，判斷是否進入紀錄狀態
myP1.GetRecStatus(Status)
Loop Until Status=1
myM1.SpdCtrlOn(0)
'啟動PID速度控制
Do 'Do迴圈，判斷是否偵測到開始點
myP1.GetRecStatus(Status)
Loop Until Status=2
myM1.SetStraight(CRAZY_SPEED_L, CRAZY_SPEED_R)
'設定高速
Do
myP1.GetTotalLen(LenR, LenL)
Loop Until LenL>STOP_TACH
myM1.BrakeDual()
'停止左右輪
myP1.StopRec()
'結束紀錄
Goto WAIT_BUTTON
END SUB
Peripheral myM1 As RacerM1 @ 3 '設定馬達控制模組參數名稱
Peripheral myP1 As RacerP1 @ 4 '設定多功能感測模組參數名稱
#DEFINE BRAKE_TIME
300 '設定停止時間
#DEFINE CROSS_TIME
20
'設定交叉軌道判斷時間
#DEFINE PID_P 17 'M1模組內部PID參數值 (0 ~ 255)
#DEFINE PID_I 0
#DEFINE PID_D 115
#DEFINE PID_SCALE 0
'模組內部最高最低速限，以及直線速度 (-1024 ~ 1024)
#DEFINE MAX_SPEED_L
1024
#DEFINE MAX_SPEED_R
1024
#DEFINE CENTER_SPEED_L 220
#DEFINE CENTER_SPEED_R 220
#DEFINE MIN_SPEED_L
-1024
#DEFINE MIN_SPEED_R
-1024
#DEFINE CRAZY_SPEED_L 450 '設定高速與開始剎車距離
#DEFINE CRAZY_SPEED_R 450
#DEFINE STOP_TACH
100
#DEFINE ERROR_1 1 '模組各別感測階段的誤差值 (0 ~ 127)
#DEFINE ERROR_2 3
#DEFINE ERROR_3 7
#DEFINE ERROR_4 15
#DEFINE ERROR_5 25
#DEFINE ERROR_6 37
SUB InitM1() '設定M1模組各項預設值
myM1.SetOutsideMode(2)
'設定離開軌道的動作
myM1.SetP(PID_P) '設定M1模組內部PID參數值
myM1.SetI(PID_I)
myM1.SetD(PID_D)
myM1.SetScalar(PID_SCALE)
'設定M1模組內部最高最低速限，以及直線速度
myM1.SetSpdCtrlA(MIN_SPEED_L, MAX_SPEED_L)
myM1.SetSpdCtrlB(MIN_SPEED_R, MAX_SPEED_R)
myM1.SetStraight(CENTER_SPEED_L, CENTER_SPEED_R)
'設定M1模組各別感測階段的誤差值
myM1.SetErrScale(ERROR_1, ERROR_2, ERROR_3, ERROR_4, ERROR_5, ERROR_6)
End Sub
InnoRacer整合控制
IR紅外線感測器
CH
數位
ST
數位
數位
數位
類比
BC2
USB
CMDBUS
按鈕及
LED燈號
操作
馬達速度控制晶
片M1
跑道記憶控
制晶片P1
EEPROM
記憶體
加速度感
測器
馬達驅動
晶片
馬達(左)
Motor
馬達驅動
晶片
編碼器
(左)
Encoder
編碼器
(右)
Encoder
馬達(右)
Motor
Buzzer發聲
元件
實驗十四: 繞圈加速整合控制
1.
2.
3.
4.
請直接參考inb 檔
試著調整各項參數
Button 2 跑道紀錄
Button 1 開始競速
如何才能讓車子跑的快?
如何才能讓車子跑的快?
1. 足夠的速度
2. 適當的PD設定
3. 適當的Error比例
4. 將跑道資訊充份利用
5. 更多段的變速
6. 足夠的磨擦力
7. 跑道與輪子的清潔
8. 適當的輪距與軸距
9. 更輕的電池
10. 適當的配重
革命尚未成功 同志仍需努力
The
END