; ; パルスモーターコントローラー ; DEVICE PIC16F84,HS_OSC,WDT_OFF,PWRT_ON,PROTECT_OFF ID 'PMCC' TXO = RA.2 ; シリアル通信入出力 RXI = RA.3 ; RDY = RA.4 ; PA1 = RB.0 ; モーター駆動出力（MODE 0,1,2） PA2 = RB.1 ; PB1 = RB.2 ; PB2 = RB.3 ; P_FWD = RB.0 ; パルス出力（MODE 3） P_REV = RB.1 ; SW_FL = RB.4 ; リミットスイッチ入力 SW_RL = RB.5 ; SW_ORG = RA.0 ; ;*************************************************************************** ; 変数エリア ; org 0Ch position ds 2 ; 絶対位置 step_remain ds 2 ; 移動先までの距離 step_moved ds 2 ; 移動した距離 phase ds 1 ; Motor phase accel ds 1 ; 加速度*16+15 (15,31,47,63,..255) velocity ds 1 ; 最高速度 (0..31) dmode ds 1 ; 励磁モード*8 (0,8,16,24) flags ds 1 ; 動作フラグ類 DIR = flags.0 ; Step direction (0:Forword, 1:Reverse) OVR = flags.1 ; Over run occured. STEPED = flags.2 ; Step interrupted. tmp ds 8 ; Temporaly registers reg_sav ds 2 ; Register save area for interrupt service routine. sioreg ds 3 ; SIO send/receive register, work area. value ds 2 ; rcvr_num's return ;*************************************************************************** ; EEPROMエリア ; eeorg 0 ; 速度テーブル ; level, pps eedata -235 ; 0, 166 eedata -195 ; 1, 200 eedata -168 ; 2, 233 eedata -147 ; 3, 266 eedata -130 ; 4, 300 eedata -117 ; 5, 333 eedata -107 ; 6, 366 eedata -98 ; 7, 400 eedata -90 ; 8, 433 eedata -84 ; 9, 466 eedata -78 ; 10, 500 eedata -73 ; 11, 533 eedata -69 ; 12, 566 eedata -65 ; 13, 600 eedata -62 ; 14, 633 eedata -59 ; 15, 700 eedata -56 ; 16, 733 eedata -53 ; 17, 766 eedata -51 ; 18, 800 eedata -49 ; 19, 833 eedata -47 ; 20, 866 eedata -45 ; 21, 900 eedata -43 ; 22, 933 eedata -42 ; 23, 966 eedata -40 ; 24, 1000 eedata -39 ; 25, 1033 eedata -38 ; 26, 1066 eedata -37 ; 27, 1100 eedata -36 ; 28, 1133 eedata -34 ; 29, 1166 eedata -33 ; 30, 1200 eedata -33 ; 31, 1266 ee_accel eedata 127 ; accel 初期値 (15,31,47,63,...,255) ee_velocity eedata 20 ; velocity 初期値 (0..31) ee_dmode eedata 0 ; dmode 初期値 (0,8,16,24) ;*************************************************************************** ; プログラムコード ; ;-----------------------------------------------------------------------------; ; リセットエントリ org 0 jmp start ;-----------------------------------------------------------------------------; ; 割り込みエントリ(TMR0のみ) org 4 interrupt mov reg_sav,w ; Save reg.w swapf status,w ; Save status mov reg_sav+1,w ; call read_epr ; Set step timer value. (speed) mov TMR0,w ; clrb T0IF ; Clear IRQ mov w,step_remain ; step_remain == 0 ? or w,step_remain+1 ; jz int_stop ; yes, end of move. call step ; Execute one step. jb OVR,int_stop ; Over run ? Yes, end of move. sub step_remain,#1 ; step_remain -- sb C ; dec step_remain+1 ; inc step_moved ; step_moved ++ snb Z ; inc step_moved+1 ; skip ; Skip next (Continue timer interrupt). int_stop clrb T0IE ; Stop timer interrupt. setb STEPED ; Set step flag. swapf reg_sav+1,w ; Restore status mov status,w ; swapf reg_sav,f ; Restore reg.w swapf reg_sav,w ; reti ; End of interrupt. ;-----------------------------------------------------------------------------; ; モーター駆動出力 ; ; Call : DIR = 0:Forword, 1:Reverse. (only "step" routine) ; Return: OVR = 0:Successful, position = updated. ; 1:Over run occured (not moved). step ; １ステップ jb DIR,step_rev ; Direction ? step_fwd ; 進める jb SW_FL,step_lim ; Forword limit SW ? inc phase inc position ; position ++ snb Z ; inc position+1 ; jmp step_drv step_rev ; 戻す jb SW_RL,step_lim ; Reverse limit SW ? dec phase mov w,position ; position -- snb Z ; dec position+1 ; dec position ; step_drv mov RB,#0 call getph mov RB,w clrb OVR ret step_lim setb OVR ret getph mov w,phase and w,#111b or w,dmode jmp pc+w retw 0101b,0110b,1010b,1001b,0101b,0110b,1010b,1001b ; 2P retw 0001b,0100b,0010b,1000b,0001b,0100b,0010b,1000b ; 1P retw 0001b,0101b,0100b,0110b,0010b,1010b,1000b,1001b ; 1-2P jmp pulse ; Pulse jmp pulse jmp pulse jmp pulse jmp pulse jmp pulse jmp pulse pulse jb DIR,pls_fwd ; Direction ? setb P_FWD skip pls_fwd setb P_REV mov w,#-18 addlw 1 jnz $-1 ret ;-----------------------------------------------------------------------------; ; １文字受信／送信 (N81 38400bps, @10MHz) ; ; Call : sioreg(0) = Transmit data ; Return: sioreg(0) = Received data ; sioreg(1-2) = Broken (work area) rx_data mov sioreg+1,#8 clrb RDY snb RXI ; Wait for RXI rise edge. jmp $-1 ; sb RXI ; jmp $-1 ; call whb ; Delay half bit time. :loop1 call wfb ; Delay full bit time. nop ; clc ; sb RXI ; Read bit. stc ; rr sioreg ; djnz sioreg+1,:loop1 ; setb RDY ret tx_data call wfb ; Pre wait. setb TXO ; Send start bit mov sioreg+1,#9 ; :loop1 call wfb ; Delay full bit time. rr sioreg ; Send data bits. btfss C ; bsf TXO ; btfsc C ; bcf TXO ; djnz sioreg+1,:loop1 ; ret whb mov sioreg+2,#9 ; Delay half bit time. djnz sioreg+2,$ ; ret ; wfb mov sioreg+2,#17 ; Delay full bit time. djnz sioreg+2,$ ; stc ; ret ; ;-----------------------------------------------------------------------------; ; １０進文字列送信 (value→) ; ; Call : value(0-1) = Value for transmit (16bit) ; Return : value(0-5),FSR = Broken xmit_num mov FSR,#tmp+1 :loop1 call xmdiv10 inc FSR mov w,value or w,value+1 jnb Z,:loop1 :loop2 dec FSR mov sioreg,INDF call tx_data cjne FSR,#tmp+1,:loop2 ret xmdiv10 mov tmp,#16 clr INDF :loop clc rl value rl value+1 rl INDF cjb INDF,#10,:skip setb value.0 sub INDF,#10 :skip djnz tmp,:loop add INDF,#'0' ret ;-----------------------------------------------------------------------------; ; １０進文字列受信 (value←) ; ; Return : value(0,1) = Received data (16bit) ; tmp(0,1) = Broken rcvr_num clr value clr value+1 :loop call rx_data sub sioreg,#'0' sb C ret csbe sioreg,#9 ret call rcn_sft mov tmp,value mov tmp+1,value+1 call rcn_sft call rcn_sft add value,tmp snb C inc value+1 add value+1,tmp+1 add value,sioreg snb C inc value+1 jmp :loop rcn_sft clc rl value rl value+1 ret ;-----------------------------------------------------------------------------; ; EEPROM読み出し／書き込み read_epr setb RP0 setb RD clrb RP0 mov w,EEDATA ret write_epr setb RP0 setb WREN mov EECON2,#55h mov EECON2,#0aah setb WR jb WR,$ clrb WREN clrb RP0 ret ;-----------------------------------------------------------------------------; ; 初期化 start clr RA clr RB setb RP0 ; Bank1 mov TRISA,#01011b ; TXO,RDY=out mov TRISB,#0f0h ; RB<3:0>=out mov OPTION,#10000101b ; タイマクロック = 64:1 clrb RP0 ; Bank0 setb GIE mov EEADR,#ee_accel ; 加速度値読み出し call read_epr ; mov accel,w ; mov EEADR,#ee_velocity; 最高速度読み出し call read_epr ; mov velocity,w ; mov EEADR,#ee_dmode ; 励磁モード読み出し call read_epr ; mov dmode,w ; clr position ; Position = 0 clr position+1 ; ; 初期化終了。実行ループへ mov w,#'9' ; 起動ステータス ;-----------------------------------------------------------------------------; ; コマンド受信／実行ループ main mov sioreg,w ; ステータス送信 call tx_data ; call rx_data ; コマンド受信 cjb sioreg,#' ',main_nd ; CR? cje sioreg,#'>',c_fwd1 ; １ステップ送り？ cje sioreg,#'<',c_rev1 ; １ステップ戻し？ cjb sioreg,#'a',$+6 ; CAPS sub sioreg,#20h ; mov tmp+4,sioreg ; コマンド待避 call rcvr_num ; 数値受信 call wfb cje tmp+4,#'G',c_move ; 絶対移動？ cje tmp+4,#'+',c_forword ; 相対移動（送り）？ cje tmp+4,#'-',c_reverse ; 相対移動（戻し）？ cje tmp+4,#'P',c_point ; 位置カウンタ設定？ cje tmp+4,#'V',c_velocity ; 最高速度設定？ cje tmp+4,#'A',c_accel ; 加速度設定？ cje tmp+4,#'M',c_dmode ; 励磁モード設定？ cje tmp+4,#'D',c_power ; 励磁 ON/OFF？ cje tmp+4,#'R',c_return ; 原点復帰？ cje tmp+4,#'?',c_stat ; ステータス表示？ cje tmp+4,#'_',c_store ; 動作パラメータ保存 main_nd mov w,#'1' ; 無効コマンド jmp main main_ok mov w,#'0' ; 正常終了 jmp main err_ov mov w,#'2' ; オーバーラン jmp main break clrb T0IE ; 強制停止 mov w,#'4' jmp main ;-----------------------------------------------; ; 励磁 ON/OFF c_power mov RB,#0 ; Drive output turn off. snb value.0 ; value.0 == 0 ? call step_drv ; no, Turn on. jmp main_ok ;-----------------------------------------------; ; 励磁モード設定 c_dmode mov dmode,value rl dmode rl dmode rl dmode and dmode,#11000b call step_drv jmp main_ok ;-----------------------------------------------; ; 動作パラメータ保存 c_store mov EEDATA,velocity mov EEADR,#ee_velocity call write_epr mov EEDATA,dmode mov EEADR,#ee_dmode call write_epr mov EEDATA,accel mov EEADR,#ee_accel call write_epr jmp main_ok ;-----------------------------------------------; ; 最高速度設定 c_velocity mov w,value and w,#31 mov velocity,w jmp main_ok ;-----------------------------------------------; ; 加速度設定 c_accel mov accel,value swap accel or accel,#15 jmp main_ok ;-----------------------------------------------; ;ポジションカウンタ値設定 c_point mov position,value mov position+1,value+1 jmp main_ok ;-----------------------------------------------; ;１ステップ送り／戻し c_fwd1 call step_fwd jb OVR,err_ov ; Over run ? jmp main_ok c_rev1 call step_rev jb OVR,err_ov ; Over run ? jmp main_ok ;-----------------------------------------------; ; 原点復帰 c_return :loop_r jb RXI,break ; Break detect ? jb SW_ORG,:loop_f setb DIR call step jb OVR,err_ov call dly jmp :loop_r :loop_f call dly jb RXI,break ; Break detect ? jnb SW_ORG,:loop_e clrb DIR call step jb OVR,err_ov jmp :loop_f :loop_e mov position,value ; Set position. mov position+1,value+1 ; jmp main_ok dly mov TMR0,#-197 ; wait 1/200s cjne TMR0,#0,$ ret ;-----------------------------------------------; ;相対移動 c_forword clrb DIR skip c_reverse setb DIR jmp auto_run ;-----------------------------------------------; ;絶対移動 c_move clrb DIR ; Set forword mode. sub value,position ; value -= position sb C ; dec value+1 ; sub value+1,position+1 ; jnb value+1.7,auto_run ; value < 0 ? no, start not value ; value = -value not value+1 ; inc value ; snb Z ; inc value+1 ; setb DIR ; Set reverse mode. jmp auto_run ; Start ;-----------------------------------------------; ; 移動開始と速度制御 auto_run mov step_remain,value ; step_remain = value (steps to move) mov step_remain+1,value+1 ; mov step_moved,#1 ; step_moved = 1 (step count) clr step_moved+1 ; mov EEADR,#0 ; Set initial velocity (0). clrb STEPED ; Clear step flag clrb T0IF ; Start timer interrupt. setb T0IE ; :loop jb RXI,break ; Break detect ? jnb STEPED,:loop ; ステップ待ち clrb STEPED ; Clear step flag jb OVR,err_ov ; Over run ? jnb T0IE,main_ok ; End of move ? mov value,step_moved ; step_remain と step_moved mov value+1,step_moved+1 ; を比較して小さい方を value subwf step_remain+1,w ; に設定 jnb Z,:cmp1 ; movf value,w ; subwf step_remain,w ; :cmp1 jb C,:go_cmpr ; mov value,step_remain ; mov value+1,step_remain+1 ; :go_cmpr mov EEADR,velocity ; 最高速度 mov tmp,accel ; 加減速値初期値 clr tmp+1 ; mov tmp+2,#5 ; 加減速距離初期値 clr tmp+3 ; clr tmp+4 ; 比較速度レベル(0..30) :lp_cmpr movf value+1,w ; valueがその速度レベル subwf tmp+3,w ; の加減速距離内なら :lpc_setへ jnb Z,:lpc_1 ; movf value,w ; subwf tmp+2,w ; :lpc_1 jb C,:lpc_set ; add tmp,accel ; 距離加算値に加速度値を加算 snb C ; inc tmp+1 ; add tmp+2,tmp+1 ; 加減速距離加算 snb C ; inc tmp+3 ; inc tmp+4 ; 次の速度レベル cjne tmp+4,#31,:lp_cmpr ; 加減速範囲外なら最高速度とする jmp :loop ; :lpc_set cjb EEADR,tmp+4,:loop ; 速度レベルを設定して抜ける mov EEADR,tmp+4 ; jmp :loop ; ;-----------------------------------------------; ; ステータス表示 c_stat mov sioreg,#'{' ; { call tx_data mov sioreg,#'P' ; P call tx_data mov value,position ; Value of position counter mov value+1,position+1 call xmit_num mov sioreg,#',' ; , call tx_data mov sioreg,#'V' ; V call tx_data mov EEADR,#ee_velocity ; Value of maximum velocity call read_epr mov value,w call xmit_num mov sioreg,#',' ; , call tx_data mov sioreg,#'A' ; A call tx_data mov EEADR,#ee_accel ; Value of acceleration call read_epr mov value,w swap value and value,#0fh call xmit_num mov sioreg,#',' ; , call tx_data mov sioreg,#'M' ; M value call tx_data mov value,dmode rr value rr value rr value and value,#3 call xmit_num mov sioreg,#'}' ; } call tx_data jmp main_ok ;-----------------------------------------------------------------------------; end