' {$STAMP BS2} ' {$PBASIC 2.5} '可利用平板電腦的藍芽訊號或者RS-232串列訊號來進行機器人的動作控制 '當使用其中一種控制模式時，便不再理會另一控制器的訊號，除非將正在使用中的控制器關閉 '對SSC-32的鮑率為38.4K '藍芽的鮑率為9.6K '藍芽訊號對應的動作： "9" → 開機、待機 ' "1" → 前進 ' "2" → 後退 ' "3" → 右轉 ' "4" → 左轉 ' "5" → 自動模式 '平板電腦在開啟控制程式時會不斷的送出訊號，所以只要關閉控制程式後，機器人也會自動關機 ' ' 'RS-232訊號對應的動作： "7" → 開機 ' "1" → 前進 ' "2" → 後退 ' "3" → 右轉 ' "4" → 左轉 ' "8" → 關機 '由於RS-232的控制方式是採用Button的方式，所以一次只會送出一個訊號，動作也只做一次 '若要做出連續動作，則必須連點鍵盤上的Button才行 ' ' ' ' '---------------------------------------------------------------------------------------------------- time VAR Word '超音波回傳值 cmDistance VAR Word '機器人前方距離 DL VAR Word '機器人左方距離 DR VAR Word '機器人右方距離 idx VAR Word '標示DATA位址的變數 idNum VAR Byte 'SSC-32的PIN腳變數 score VAR Word '儲存DATA中伺服馬達的值 position VAR Byte '將score加乘運算後適合SSC-32的數值 a VAR Byte 'DATA編號變數 b VAR Byte 'DATA位址變數 d VAR Byte '關節DATA位址 i VAR Byte 'DATA起始位置 r VAR Byte 'DATA終端位置 e VAR Byte '藍芽目前狀態變數函數 g VAR Byte '藍芽前一狀態變數函數 h VAR Byte 'RS-232變數函數 RX CON 15 ' HomeWork Board PIN腳對應EasyBT藍芽RX腳位 TX CON 14 ' HomeWork Board PIN腳對應EasyBT藍芽TX腳位 CmConstant CON 2260 '-------動作參數------------------------------------------------------------------------------------- '以下DATA的參數皆為一個伺服馬達的編號對應一個伺服馬達的轉角數值 '伺服馬達的數值是原本SSC-32的值先減1000再除5之後得出的結果 '例如： (1500-1000)/5 = 100 cLF DATA 0, 100, 1, 140, 2, 72, '左前腳 前進 0, 66, 1, 140, 2, 72, 0, 66, 1, 97, 2, 72, 0, 76, 1, 99, 2, 110, 0, 96, 1, 90, 2, 134, 0, 96, 1, 90, 2, 134, 0, 100, 1, 140, 2, 119, '後退 0, 136, 1, 140, 2, 119, 0, 136, 1, 90, 2, 119, 0, 110, 1, 100, 2, 93, 0, 94, 1, 92, 2, 54, 0, 94, 1, 92, 2, 54, 0, 100, 1, 140, 2, 100, '右轉 0, 79, 1, 140, 2, 100, 0, 79, 1, 100, 2, 100, 0, 100, 1, 100, 2, 99, 0, 121, 1, 100, 2, 100, 0, 121, 1, 100, 2, 100, 0, 100, 1, 140, 2, 100, '左轉 0, 121, 1, 140, 2, 100, 0, 121, 1, 100, 2, 100, 0, 100, 1, 100, 2, 99, 0, 79, 1, 100, 2, 100, 0, 79, 1, 100, 2, 100 cLM DATA 4, 100, 5, 100, 6, 100, '左中腳 前進 4, 136, 5, 100, 6, 95, 4, 136, 5, 100, 6, 95, 4, 100, 5, 140, 6, 95, 4, 64, 5, 140, 6, 95, 4, 64, 5, 100, 6, 95, 4, 100, 5, 100, 6, 100, '後退 4, 64, 5, 100, 6, 95, 4, 64, 5, 100, 6, 95, 4, 100, 5, 140, 6, 95, 4, 136, 5, 140, 6, 95, 4, 136, 5, 100, 6, 95, 4, 100, 5, 100, 6, 99, '右轉 4, 118, 5, 100, 6, 100, 4, 118, 5, 100, 6, 100, 4, 100, 5, 140, 6, 100, 4, 82, 5, 140, 6, 100, 4, 82, 5, 100, 6, 100, 4, 100, 5, 100, 6, 101, '左轉 4, 82, 5, 100, 6, 100, 4, 82, 5, 100, 6, 100, 4, 100, 5, 140, 6, 100, 4, 118, 5, 140, 6, 100, 4, 118, 5, 100, 6, 100 cLR DATA 8, 100, 9, 140, 10, 119, '左後腳 前進 8, 64, 9, 140, 10, 119, 8, 64, 9, 98, 10, 119, 8, 90, 9, 100, 10, 93, 8, 106, 9, 92, 10, 54, 8, 106, 9, 92, 10, 54, 8, 100, 9, 140, 10, 72, '後退 8, 134, 9, 140, 10, 72, 8, 134, 9, 94, 10, 76, 8, 124, 9, 99, 10, 110, 8, 104, 9, 90, 10, 134, 8, 104, 9, 90, 10, 134, 8, 100, 9, 140, 10, 100, '右轉 8, 79, 9, 140, 10, 100, 8, 79, 9, 100, 10, 100, 8, 100, 9, 100, 10, 99, 8, 121, 9, 100, 10, 100, 8, 121, 9, 100, 10, 100, 8, 100, 9, 140, 10, 100, '左轉 8, 121, 9, 140, 10, 100, 8, 121, 9, 100, 10, 100, 8, 100, 9, 100, 10, 99, 8, 79, 9, 100, 10, 100, 8, 79, 9, 100, 10, 100 cRF DATA 16, 124, 17, 101, 18, 90, '右前腳 前進 16, 104, 17, 110, 18, 66, 16, 104, 17, 110, 18, 66, 16, 100, 17, 60, 18, 128, 16, 134, 17, 60, 18, 128, 16, 134, 17, 123, 18, 128, 16, 90, 17, 100, 18, 107, '後退 16, 106, 17, 108, 18, 146, 16, 106, 17, 108, 18, 146, 16, 100, 17, 60, 18, 81, 16, 64, 17, 60, 18, 81, 16, 64, 17, 102, 18, 81, 16, 100, 17, 100, 18, 101, '右轉 16, 121, 17, 100, 18, 100, 16, 121, 17, 100, 18, 100, 16, 100, 17, 60, 18, 100, 16, 79, 17, 60, 18, 100, 16, 79, 17, 100, 18, 100, 16, 100, 17, 100, 18, 101, '左轉 16, 79, 17, 100, 18, 100, 16, 79, 17, 100, 18, 100, 16, 100, 17, 60, 18, 100, 16, 121, 17, 60, 18, 100, 16, 121, 17, 100, 18, 100 cRM DATA 20, 100, 21, 60, 22, 105, '右中腳 前進 20, 136, 21, 60, 22, 105, 20, 136, 21, 100, 22, 105, 20, 100, 21, 100, 22, 100, 20, 64, 21, 100, 22, 105, 20, 64, 21, 100, 22, 105, 20, 100, 21, 60, 22, 105, '後退 20, 64, 21, 60, 22, 105, 20, 64, 21, 100, 22, 105, 20, 100, 21, 100, 22, 100, 20, 136, 21, 100, 22, 105, 20, 136, 21, 100, 22, 105, 20, 100, 21, 60, 22, 100, '右轉 20, 82, 21, 60, 22, 100, 20, 82, 21, 100, 22, 100, 20, 100, 21, 100, 22, 101, 20, 118, 21, 100, 22, 100, 20, 118, 21, 100, 22, 100, 20, 100, 21, 60, 22, 100, '左轉 20, 118, 21, 60, 22, 100, 20, 118, 21, 100, 22, 100, 20, 100, 21, 100, 22, 101, 20, 82, 21, 100, 22, 100, 20, 82, 21, 100, 22, 100 cRR DATA 24, 110, 25, 100, 26, 107, '右後腳 前進 24, 94, 25, 108, 26, 146, 24, 94, 25, 108, 26, 146, 24, 100, 25, 60, 26, 81, 24, 136, 25, 60, 26, 81, 24, 136, 25, 102, 26, 81, 24, 76, 25, 101, 26, 90, '後退 24, 96, 25, 110, 26, 66, 24, 96, 25, 110, 26, 66, 24, 100, 25, 60, 26, 128, 24, 66, 25, 60, 26, 128, 24, 103, 25, 103, 26, 128, 24, 100, 25, 100, 26, 101, '右轉 24, 121, 25, 100, 26, 100, 24, 121, 25, 100, 26, 100, 24, 100, 25, 60, 26, 100, 24, 79, 25, 60, 26, 100, 24, 79, 25, 100, 26, 100, 24, 100, 25, 100, 26, 101, '左轉 24, 79, 25, 100, 26, 100, 24, 79, 25, 100, 26, 100, 24, 100, 25, 60, 26, 100, 24, 121, 25, 60, 26, 100, 24, 121, 25, 100, 26, 100 '-----------啟動訊號輸入----------------------------------------------------------------------------------- start_signal1: SERIN 16,84,990,start_signal2, [STR h\1] '等待RS-232指令，若時間內無訊號輸入，便跳到start_signal2 IF (h = "7") THEN '輸入訊號為字串7，進入啟動程序，若非則重新等待訊號 GOSUB start ENDIF GOTO input_signal1 start_signal2: SERIN TX,84,10,start_signal1, [STR e\1] '等待藍芽訊號，若時間內無訊號輸入，便跳回start_signal1 IF (e = "9") THEN '輸入訊號為字串9，進入啟動程序，若非則重新等待訊號 GOSUB start ENDIF GOTO input_signal2 '----------啟動動作，使六足呈現站立姿勢-------------------------------------------------------------------- start: FOR a = 0 TO 26 SEROUT 0, 6, ["#",DEC a,"P1500"] NEXT SEROUT 0, 6, ["T1000",13] PAUSE 1000 IF (h<>0) THEN GOSUB StartLeg ENDIF SEROUT 0, 6, ["#15 P1570 T200",13] '使超音波偵測器朝向前方 RETURN '-----------動作訊號輸入----------------------------------------------------------------------------------- input_signal1: '進入RS-232控制模式，此時不理會藍芽訊號 SERIN 16,84, [STR h\1] IF (h = "8") THEN '若輸入"8"，則關閉六足關節的電源 GOTO stopmove ENDIF GOTO main input_signal2: '進入藍芽控制模式，此時不理會RS-232指令 SERIN TX,84,100,stopmove, [STR e\1] '若無訊號輸入，則關閉六足關節的電源 g = e '紀錄目前狀態 GOTO main '-----------判別輸入訊號選擇動作--------------------------------------------------------------------------- main: IF (e = "1") OR (e = "5") OR (h = "1") THEN '前進 i = 0 r = 30 ELSEIF (e = "2") OR (h = "2") THEN '後退 i = 36 r = 66 ELSEIF (e = "3") OR (h = "3") THEN '右轉 i = 72 r = 102 ELSEIF (e = "4") OR (h = "4") THEN '左轉 i = 108 r = 138 ELSEIF e = "9" THEN GOSUB StartLeg DO WHILE (e = "9") PAUSE 500 SERIN TX, 84, 500, stopmove, [STR e\1] LOOP ENDIF IF (e <> 0) AND (e <> "9") THEN '當藍芽先啟動且下達的指令不為待機時執行 DO WHILE (e=g) GOSUB walk SERIN TX, 84, 500, stopmove, [STR e\1] LOOP GOTO input_signal2 ENDIF IF (h <> 0) THEN '當RS-232先啟動且有下達其他指令時執行 GOSUB walk SERIN 16,84,900,nex, [STR h\1] '結束一個動作時偵測是否有下一指令輸入，有的話便直接執行，若無則回到站立狀態 IF (h <> 0) THEN GOTO main ENDIF nex: GOSUB StartLeg GOTO input_signal1 ENDIF END '----------------------------------------------------------------------------------------------------------- StartLeg: '收腳動作 FOR a = 0 TO 24 STEP 4 IF (a>=16) THEN score = 1100 ELSE score = 1900 ENDIF SEROUT 0, 6, ["#",DEC a+1,"P",DEC score,"T100",13] PAUSE 100 SEROUT 0, 6, ["#",DEC a,"P1500","#",DEC a+2,"P1500 T100",13] PAUSE 100 SEROUT 0, 6, ["#",DEC a+1,"P1500 T100",13] PAUSE 100 NEXT RETURN '------------------------------------------------------------------------------------------------------------ walk: '行走動作 FOR b = i TO r STEP 6 FOR a = 0 TO 5 FOR d = 0 TO 4 STEP 2 LOOKUP a, [cLF, cLM, cLR, cRF, cRM, cRR], idx READ (idx + (b+d)), idNum, position '從DATA讀出的2個值分別存入idNum與position 'idNum代表伺服馬達的編號，position代表馬達轉動的位置 score = 1000 + (position*5) '將position加乘放大成適合SSC-32的數值後存入score SEROUT 0, 6, ["#",DEC idNum,"P",DEC score] NEXT NEXT SEROUT 0, 6, ["T250",13] 'PAUSE 50 '---------當偵測到藍芽訊號為"5"時會開啟超音波偵測器---------------------------- IF (e = "5") THEN PULSOUT 9, 5 PULSIN 9, 1, time cmDistance = CmConstant ** time SERIN TX, 84, 500, stopmove, [STR e\1] IF ((i = 0) AND (cmDistance<50)) THEN explore '前進時，前方50CM若有障礙物便進行偵測 IF ((i <> 0) AND (cmDistance>100)) THEN explore '其他動作時(左轉、右轉、後退)，若前方障礙物距離已大於1M，便再進行一次偵測 ENDIF NEXT RETURN '----------------------------------------------------------------------------------------------------------- explore: '偵測動作 PAUSE 1000 SEROUT 0, 6, ["#15 P850 T100",13] '超音波偵測右邊障礙物 PAUSE 1000 PULSOUT 9, 5 PULSIN 9, 1, time DR = CmConstant ** time PAUSE 500 SEROUT 0, 6, ["#15 P2150 T100",13] '超音波偵測左邊障礙物 PAUSE 1000 PULSOUT 9, 5 PULSIN 9, 1, time DL = CmConstant ** time PAUSE 500 SEROUT 0, 6, ["#15 P1500 T100",13] '超音波偵測前方障礙物 PAUSE 1000 PULSOUT 9, 5 PULSIN 9, 1, time cmDistance = CmConstant ** time PAUSE 500 '---------依照以上數值來決定要執行何種動作----------------------------------------------------------- IF (DL<30) AND (DR<30) THEN i = 36 '左右皆小於30cm時往後退 r = 66 ELSEIF (cmDistance>70) AND (DR>50) AND (DL>50) THEN i = 0 '前方大於70cm、左右大於50cm時往前進 r = 30 ELSEIF (DRDL) THEN i = 72 r = 102 ENDIF GOTO walk '----------關閉所有伺服馬達的電源，並跳回start_signal1重新等待指令----------------------------------- stopmove: FOR a = 0 TO 26 SEROUT 0, 6, ["#",DEC a,"P0"] NEXT SEROUT 0, 6, ["T200",13] GOTO start_signal1