プチロボXのモーション再生ができるプログラムです。
モーションスクリプトは互換性を持たせました。
HSP 3.xで動きます。
すごい長いです。
プログラムを拡張して、ジョイスティックで動かすとか、タイマーで動かすとか、応用ができるはずです。
//puchi robo x用 通信プログラム3 for HSP 3 //by takuya matsubara //http://nicotak.com #include "hspext.as" #define COMNUM 1 // ポート番号 #define MOTORMAX 9 #define POSPITCH 3 #define TATIKOMA 0 dim nowpos,MOTORMAX dim endpos,MOTORMAX dim posminus,MOTORMAX dim home,MOTORMAX sdim motion,1000 sdim homefile,12 sdim crlf,3 crlf="__" poke crlf,0,13 //CR poke crlf,1,10 //LF speed = 20 motion="" screen 0,640,480,0,0,0 comopen COMNUM,"baud=4800 parity=N data=8 stop=1" //シリアルポートを初期化 if stat : dialog "COM"+COMNUM+"は使えません" : end if (TATIKOMA) { posminus.0 = 0 posminus.1 = 0 posminus.2 = 1 posminus.3 = 1 posminus.4 = 0 posminus.5 = 0 posminus.6 = 1 posminus.7 = 1 posminus.8 = 0 motion=motion+";タチコマ"+crlf motion=motion+"S15"+crlf motion=motion+"0+60,1+60,2+00,3+35,4+20,5-10,6+20,7-60,8+15"+crlf motion=motion+"W150"+crlf motion=motion+"0+00,1+70,2+00,3+00,4+20,5-35,6+20,7-35,8+0"+crlf motion=motion+"W150"+crlf motion=motion+"0+00,1+35,2+60,3+60,4+20,5-60,6+20,7-10,8-15"+crlf motion=motion+"W150"+crlf motion=motion+"0+00,1+00,2+00,3+70,4+20,5-35,6+20,7-35,8+0"+crlf motion=motion+"W150"+crlf homefile="homepos2.bin" }else{ posminus.0 = 0 posminus.1 = 0 posminus.2 = 0 posminus.3 = 1 posminus.4 = 1 posminus.5 = 0 posminus.6 = 0 posminus.7 = 0 posminus.8 = 1 motion=motion+";人型前進"+crlf motion=motion+"S20"+crlf motion=motion+"5+10,2+10"+crlf motion=motion+"1+30,0-30,4-30,3+30,8+40,7-40"+crlf motion=motion+"2-10,5-10"+crlf motion=motion+"1-30,0+30,4+30,3-30,8-40,7+40"+crlf homefile="homepos.bin" } pos 200,0 mesbox motion,300,180 objsize 120,20 tmpy=25 pos 10,tmpy:tmpy += 25 button gosub "load motion",*loadmotion pos 10,tmpy:tmpy += 25 button gosub "play motion",*playmotion pos 10,tmpy:tmpy += 25 button gosub "set home",*sethome pos 10,tmpy:tmpy += 25 button gosub "save home",*homesave objsize 50,20 tmpy = 200 pos 60*0+400,tmpy:button gosub "free",*free0 pos 60*1+400,tmpy:button gosub "pos+",*pos0_add pos 60*2+400,tmpy:button gosub "pos-",*pos0_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free1 pos 60*1+400,tmpy:button gosub "pos+",*pos1_add pos 60*2+400,tmpy:button gosub "pos-",*pos1_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free2 pos 60*1+400,tmpy:button gosub "pos+",*pos2_add pos 60*2+400,tmpy:button gosub "pos-",*pos2_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free3 pos 60*1+400,tmpy:button gosub "pos+",*pos3_add pos 60*2+400,tmpy:button gosub "pos-",*pos3_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free4 pos 60*1+400,tmpy:button gosub "pos+",*pos4_add pos 60*2+400,tmpy:button gosub "pos-",*pos4_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free5 pos 60*1+400,tmpy:button gosub "pos+",*pos5_add pos 60*2+400,tmpy:button gosub "pos-",*pos5_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free6 pos 60*1+400,tmpy:button gosub "pos+",*pos6_add pos 60*2+400,tmpy:button gosub "pos-",*pos6_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free7 pos 60*1+400,tmpy:button gosub "pos+",*pos7_add pos 60*2+400,tmpy:button gosub "pos-",*pos7_sub tmpy += 25 pos 60*0+400,tmpy:button gosub "free",*free8 pos 60*1+400,tmpy:button gosub "pos+",*pos8_add pos 60*2+400,tmpy:button gosub "pos-",*pos8_sub gosub *homeload gosub *printpos stop //------------------- *posx_add nowpos.mot += POSPITCH: goto *pos_change //------------------- *posx_sub nowpos.mot -= POSPITCH: goto *pos_change *pos_change if(nowpos.mot > 220):nowpos.mot=220 if(nowpos.mot < 1) :nowpos.mot=1 position = nowpos.mot gosub *send_pos gosub *printpos return //------------------- *printpos redraw 0 tmpy = 200 for i,0,MOTORMAX color 255,255,200 boxf 0,tmpy,390,tmpy+20 color 0,0,0 pos 10,tmpy:print "motor num="+i pos 120,tmpy:print "pos="+str(nowpos.i - home.i) pos 200,tmpy:print "home="+str(home.i) tmpy += 25 next redraw 1 return //--------------------- *loadmotion dialog "msf",16,"motion" if (stat==0) : return notesel motion noteload refstr //テキストファイル読み込み objprm 0,motion return *savemotion dialog "msf",17,"motion" if( stat==0 ): return fname = refstr exname=getpath(fname,2) //ファイル名の拡張子を取得 if exname="" : fname+=".msf" //拡張子がなければ追加 notesel motion notesave fname //テキストファイルをセーブ return *sethome for mot,0,MOTORMAX nowpos.mot = home.mot position = home.mot gosub *send_pos next gosub *printpos return //-------------------モーションスクリプト再生 *playmotion for mot,0,MOTORMAX endpos.mot = nowpos.mot next ptr=0 repeat if(ptr >= strlen(motion)):break max=1:gosub *getcode //1文字読み取り if(ch==0x45){// 'E'= end break } if(ch==0x57){// 'W'= wait max=4:gosub *getcode wait val/10 continue } if(ch==0x53){// 'S'= speed max=4:gosub *getcode speed=val continue } if(ch==0x3b){// セミコロン= 注釈 while(ch!=13) max=1:gosub *getcode //1文字読み取り wend continue } if(ch < 0x30):continue if(ch > 0x39):continue mot = val //モータ番号 max=4:gosub *getcode //角度情報 if(ch == 0x46 ){ // 'F' = free motor endpos.mot = 0 }else{ val += home.mot if(val < 1):val=1 if(val > 220):val=220 endpos.mot = val } if(ch==13){ gosub *onemotion } loop return //--------------------------------- *onemotion repeat matchcnt =0 for mot,0,MOTORMAX if(endpos.mot == 0){ //free gosub *send_free matchcnt ++ }else{ distance = endpos.mot - nowpos.mot if(distance == 0){ matchcnt ++ //目的地に到着 }else{ if(distance > speed):distance = speed if(distance < -speed):distance = -speed nowpos.mot += distance position = nowpos.mot gosub *send_pos } } next if(matchcnt == MOTORMAX):break //全モータが到着 loop wait 15 gosub *printpos return //-------------------モータをフリー *send_free position=0 goto *send_possub //-------------------モータ回転制御コマンド *send_pos if(posminus.mot == 1) { //回転向きを逆にする position = 221 - position } if(position > 220):position=220 if(position < 1) :position=1 *send_possub computc 253 //第1バイト データスタート値 "253" computc 221+mot //第2バイト モータ番号値 "221〜229" computc position //第3バイト 回転位置値 "0〜220" computc mot+1+position //第4バイト チェックサム"第2バイト-220+第3バイト" return //--------------------------------- *homesave sdim buff,100 dialog "ホームポジションを保存しますか？",2 if(stat==7):return for i,0,MOTORMAX home.i = nowpos.i if(home.i > 220) :home.i = 220 if(home.i < 1 ) :home.i = 1 poke buff,i,home.i next bsave homefile,buff,9 gosub *printpos dialog "保存しました" return //--------------------------------- *homeload sdim buff,100 sdim strtmp,100 dirlist strtmp,homefile if(strtmp==""){ dialog homefile+"がみつかりません" for i,0,MOTORMAX home.i = 110 nowpos.i = 110 next return } bload homefile,buff if(strsize <= 0){ return } for i,0,MOTORMAX home.i = peek(buff,i) // if(home.i > 220): home.i = 220 // if(home.i < 0 ) :home.i = 0 // nowpos.i = home.i next gosub *sethome return //------------------------------ *getcode val = 0 minus = 0 repeat if(ptr >= strlen(motion)): break ch = peek(motion,ptr) ptr++ if(ch == 0x2c ):break // カンマ if(ch == 13 ):break // CR if(ch == 0x46 ):break // 'F' = free motor if(ch == 0x2b):continue // '+' if(ch == 10 ):continue // LF if(ch >= 0x30){ // '0'-'9' if(ch <= 0x39){ val *= 10 val += (ch - 0x30) } } if(ch = 0x2D){ // '-' minus = 1 } max-- if(max<=0):break loop if(minus){ val = -val } return //------------------- *free0 mot=0:gosub *send_free: return *pos0_add mot=0:gosub *posx_add: return *pos0_sub mot=0:gosub *posx_sub: return //------------------- *free1 mot=1:gosub *send_free: return *pos1_add mot=1:gosub *posx_add: return *pos1_sub mot=1:gosub *posx_sub: return //------------------- *free2 mot=2:gosub *send_free: return *pos2_add mot=2:gosub *posx_add: return *pos2_sub mot=2:gosub *posx_sub: return //------------------- *free3 mot=3:gosub *send_free: return *pos3_add mot=3:gosub *posx_add: return *pos3_sub mot=3:gosub *posx_sub: return //------------------- *free4 mot=4:gosub *send_free: return *pos4_add mot=4:gosub *posx_add: return *pos4_sub mot=4:gosub *posx_sub: return //------------------- *free5 mot=5:gosub *send_free: return *pos5_add mot=5:gosub *posx_add: return *pos5_sub mot=5:gosub *posx_sub: return //------------------- *free6 mot=6:gosub *send_free: return *pos6_add mot=6:gosub *posx_add: return *pos6_sub mot=6:gosub *posx_sub: return //------------------- *free7 mot=7:gosub *send_free: return *pos7_add mot=7:gosub *posx_add: return *pos7_sub mot=7:gosub *posx_sub: return //------------------- *free8 mot=8:gosub *send_free: return *pos8_add mot=8:gosub *posx_add: return *pos8_sub mot=8:gosub *posx_sub: return
