|
本帖最後由 ko3028235 於 2013-1-11 09:04 編輯
因為老師要想問一下有人可幫畫這個示範給小弟看?重來沒畫過~~壓力感測器開啟,若壓力感測器偵測到壓力時(表示物品已經放置在機器手臂上),啟動電子羅盤。
若電子羅盤偵測結果為南方(表示機器手臂移動至南方),則啟動雙軸加速器。
依據雙軸加速器偵測結果,在DEBUG視窗顯示上、下、左、右、左上、左下、右上、右下。(顯示機器手臂目前旋轉方向)
' {$STAMP BS2}
' {$PBASIC 2.5}
'-----[ Declarations ]-------------------------------------
rawForce VAR Word
sensorPin CON 0
DinDout PIN 6
Clk PIN 5
En PIN 4
Reset CON %0000
Measure CON %1000
Report CON %1100
Ready CON %1100
NegMask CON %11111000000
x VAR Word
y VAR Word
status VAR Nib
angle VAR Word
'---------------------
xg VAR Word
yg VAR Word
cher VAR Byte
index VAR Word
temp VAR Byte
'-----[ Main Routine ]---------------------------------------------
Display: '壓力感測器
HIGH 0
PAUSE 50
RCTIME sensorPin,1,rawForce
rawForce=rawForce**52
Fingl: '程式
IF (rawForce>1)THEN '震動感測到 '
DO
GOSUB Compass_Get_Axes '執行羅盤
angle = x ATN -y
angle = angle */360
DEBUG HOME,"s",DEC3 angle,"e"
PAUSE 150
'--------------------------------------------------------------
IF 45>angle>316 THEN
DEBUG "北方" ,CR
ELSEIF 135>angle>46 THEN
DEBUG "東方" ,CR
ELSEIF 225>angle>136 THEN
DEBUG "西方" ,CR
'----------------------------------------------------
ELSEIF 315>angle>226 THEN
DEBUG "南方" ,CR
FOR index = 0 TO 242 '開始執行雙軸
READ index ,cher
DEBUG cher
NEXT
DO
index=(22*yg)+xg+1
READ index ,cher
DEBUG CRSRXY, xg, yg,cher
PAUSE 50
PULSIN 11,1,xg
PULSIN 10,1,yg
xg=(xg MIN 1875 MAX 3125)- 1875**1101
yg=(yg MIN 1875 MAX 3125)- 1875**576
yg=10-yg
DEBUG CRSRXY, xg, yg, "*"
PAUSE 50
'--------------------------------------------------------------
IF(xg=10)AND(yg<5)THEN
DEBUG "上"
PAUSE 50
DEBUG CLS
ELSEIF(xg<10)AND(yg<5)THEN
DEBUG "左上"
PAUSE 50
DEBUG CLS
ELSEIF(xg<10)AND(yg=5)THEN
DEBUG "左"
PAUSE 50
DEBUG CLS
ELSEIF(xg<10)AND(yg>5)THEN
DEBUG "左下"
PAUSE 50
DEBUG CLS
ELSEIF(xg=10)AND(yg>5)THEN
DEBUG "下"
PAUSE 50
DEBUG CLS
ELSEIF(xg>10)AND(yg>5)THEN
DEBUG "右下"
PAUSE 50
DEBUG CLS
ELSEIF(xg>10)AND(yg=5)THEN
DEBUG "右"
PAUSE 50
DEBUG CLS
ELSEIF(xg>10)AND(yg<5)THEN
DEBUG "右上"
PAUSE 50
DEBUG CLS
ELSEIF(xg=10)AND(yg=5)THEN
DEBUG "中"
PAUSE 1000
DEBUG CLS
GOTO Fingl '當雙軸感測器方位中時 回復到上面程式
ENDIF
LOOP
ENDIF
LOOP
ENDIF
GOTO Display
'----------------------------------------------------------
Compass_Get_Axes:
HIGH En: LOW En
SHIFTOUT DinDout,clk,MSBFIRST,[Reset\4]
HIGH En: LOW En
SHIFTOUT DinDout,clk,MSBFIRST,[Measure\4]
status = 0
DO
HIGH En: LOW En
SHIFTOUT DinDout,clk,MSBFIRST,[Report\4]
SHIFTIN DinDout,clk,MSBPOST,[Status\4]
LOOP UNTIL status = Ready
SHIFTIN DinDout,clk,MSBPOST,[x\11,y\11]
HIGH En
IF (y.BIT10 = 1)THEN y = y |NegMask
IF (x.BIT10 = 1)THEN x = x |NegMask
RETURN |
|