プチロボXをPIC-BASICで動かす


プチロボXを秋月のPIC-BASICでコントロールするプログラムです。単にシリアル通信でコマンドを出してるだけですが、、。

プログラム内には前進と右旋回のモーションを入れてます。ホームポジション(配列変数 hm)は各人で最適値に修正してください。

SW1を押さない状態だと前進、SW1を押し続けると右旋回します。
プチロボXには足のヨー軸がありませんが、片足と片手を振る反動で方向を変えることができます。

補足しておくと、
使用したPIC-BASICは秋月で売ってるベースボードです。シリアル通信の送受信の方向が逆になっているので、プチロボ専用の通信ケーブルとつなぐ場合には、Dsub9pinオスのコネクタを2つ用意して、
・2番pin-3番pin
・3番pin-2番pin
・5番pin-5番pin
、、、こんな感じに配線して、クロスさせます。

あと、微弱無線モジュール送信機(WR-TX)を接続する場合には、微弱無線ICを動かすための電源が必要になります。自分の場合は、ベースボード用の電源である9Vを電源用端子から入れました。微弱無線モジュール送信機には三端子レギュレータが載っているので、内部は5Vに安定化されます。

'puchix_walk.pb
'プチロボX 歩行プログラム for PIC-BASIC
'by takuya matsubara (nicotak.com)

	Dim id As Byte		'モータ番号(0-8)
	Dim speed As Byte	'最大回転量
	Dim pos As Byte		'ポジション(0-220)
	Dim t1 As Byte		'コマンド
	Dim t2 As Byte		'
	Dim t3 As Byte		'
	Dim t4 As Byte		'
	Dim p1(9) As Byte	'開始ポジション
	Dim p2(9) As Byte	'終了ポジション
	Dim hm(9) As Byte	'ホームポジション
	Dim minus(9) As Byte	'回転方向
	Dim dis As Byte	'
	Dim matchcnt As Byte
	Dim temp As Word

	Serclear	'シリアルポートの初期化
	Initlcd
	Putlcd	"Puchi Robo X"
	Sleep 2000
	Clearlcd

	speed=20	'モータの最大回転量

	minus(0)=0	'回転方向(0=プラス/1=マイナス)
	minus(1)=0
	minus(2)=0
	minus(3)=1
	minus(4)=1
	minus(5)=0
	minus(6)=0
	minus(7)=0
	minus(8)=1

	'-----モーターの初期ポジションを設定
	For id=0 To 8
		p1(id)=128
		p2(id)=128
	Next

	'-----モーターのホームポジションを設定
	hm(0)=107
	hm(1)= 98
	hm(2)= 98
	hm(3)=125
	hm(4)= 98
	hm(5)=113
	hm(6)=107
	hm(7)=107
	hm(8)= 95

	For id=0 To 8
		pos = hm(id)
		Gosub send_pos2
	Next

	Putlcd "Push Sw1"
	Gosub keywait	'スイッチ入力待ち
	Sleep 1000

	'------歩行開始
main:
	While (rb.Bit0 = 0)
		Gosub turnright	'Sw1を押すと右旋回
	Wend

'------前進
walking:
	p2(2)=128 +10	'重心移動
	p2(5)=128 +10
	Gosub setsurvo

	p2(0)=128 -30	'手足を前に出す
	p2(1)=128 +30
	p2(3)=128 +30
	p2(4)=128 -30
	p2(7)=128 -40
	p2(8)=128 +40
	Gosub setsurvo

	p2(2)=128 -10	'重心移動
	p2(5)=128 -10
	Gosub setsurvo

	p2(0)=128 +30	'手足を前に出す
	p2(1)=128 -30
	p2(3)=128 -30
	p2(4)=128 +30
	p2(7)=128 +40
	p2(8)=128 -40
	Gosub setsurvo

	Sleep 2000
	Goto main	'動作に戻る

turnright:			'右旋回
	For id=0 To 8
		p2(id)=128
	Next

	p2(7)=128 +64	'手を上げる
	Gosub setsurvo

	p2(0)=128 -10	'片足で蹴る
	p2(2)=128 -45
	p2(3)=128 +35
	Gosub setsurvo

	p2(0)=128		'手足を戻す
	p2(2)=128
	p2(3)=128
	p2(7)=128
	Gosub setsurvo

	Return

'-------------サーボモータ駆動
setsurvo:
	Clearlcd
	matchcnt =0 

	For id=0 To 8
'		Setpos (id Mod 4)*4,id / 4
'		Putlcd p2(id),"."

		If p2(id)=p1(id) Then
			matchcnt = matchcnt+1
		Else	
			If p2(id)>p1(id) Then '+方向	
				dis = p2(id)-p1(id)
				If dis > speed Then dis = speed
				p1(id) = p1(id) +dis
			Else				'-方向
				dis = p1(id)-p2(id)
				If dis > speed Then dis = speed
				p1(id) = p1(id) -dis
			Endif

			temp = p1(id)
			temp = temp + hm(id)-128
			pos = temp
			Gosub send_pos2
		Endif
	Next
	If matchcnt<9 Then Goto setsurvo

'	GOSUB keywait	'動作チェック用
	Sleep 100
	Return
'---------------スイッチ入力待ち
keywait:
	If (rb.Bit0 = 1) Then Goto keywait
	Return

'---------------モータ制御コマンド送信
'(引数)id=モータ番号、pos=ポジション
send_pos2:
	If minus(id) Then pos = 221 - pos
send_pos:
	If (id > 8)  Then Return	'引数エラーチェック
	If (pos > 220) Then  Return

	t1 = 253
	t2 = 221+id 'モータ番号値
	t3 = pos 'ポジション0220(0=モータOFF)
	t4 = (t2 - 220) + t3	'sum
	Serout pb4800,chr$(t1),chr$(t2),chr$(t3),chr$(t4)	'コマンド送信
	Return