Robofun 機器人論壇

標題: Stepper Motor controller [打印本頁]

作者: uucww 時間: 2006-8-16 13:16
標題: Stepper Motor controller
Stepper Motor controller

http://www.interq.or.jp/japan/se-inoue/e_step.htm
http://www.interq.or.jp/japan/se-inoue/e_step4.htm

step1.jpg (24.16 KB, 下載次數: 521)

作者: uucww 時間: 2006-8-16 13:21
001
002
003
004
005
006
007
008
009
010
011
012
013
014
015
016
017
018
019
020
021
022
023
024
025
026
027
028
029
030
031
032
033
034
035
036
037
038
039
040
041
042
043
044
045
046
047
048
049
050
051
052
053
054
055
056
057
058
059
060
061
062
063
064
065
066
067
068
069
070
071
072
073
074
075
076
077
078
079
080
081
082
083
084
085
086
087
088
089
090
091
092
093
094
095
096
097
098
099
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
;********************************************************
;
;             Stepper Motor controller
;
;                               Author : Seiichi Inoue
;********************************************************

      list          p=pic16f84a
      include       p16f84a.inc
      __config _hs_osc & _wdt_off & _pwrte_on & _cp_off

;****************  Label Definition  ********************
      cblock  h'0c'
mode                         ;Operation mode
                              ;0=stop 1=right 2=left
count1                         ;Wait counter
count2                         ;Wait counter(for 1msec)
      endc

rb0    equ    0             ;RB0 of PORTB
rb1    equ    1             ;RB1 of PORTB
rb2    equ    2             ;RB2 of PORTB
rb5    equ    5             ;RB5 of PORTB
rb7    equ    7             ;RB7 of PORTB

;****************  Program Start  ***********************
      org    0             ;Reset Vector
      goto init
      org    4             ;Interrupt Vector
      clrf intcon       ;Clear Interruption reg

;****************  Initial Process  *********************
init
      bsf    status,rp0    ;Change to Bank1
      clrf trisa          ;Set PORTA all OUT
      movlw b'00100111'    ;RB0,1,2.5=IN RB7=OUT
      movwf trisb          ;Set PORTB
      movlw b'10000000'    ;RBPU=1 Pull up not use
      movwf option_reg    ;Set OPTION_REG
      bcf    status,rp0    ;Change to Bank0
      clrf mode          ;Set mode = stop
      clrf count1       ;Clear counter
      clrf count2       ;Clear counter
      movlw b'00000101'    ;Set PORTA initial value
      movwf porta          ;Write PORTA
      bsf    portb,rb7    ;Set RB7 = 1
      btfsc portb,rb5    ;RB5 = 0 ?
      goto $-1          ;No. Wait

start
;*************  Check switch condition  *****************
      btfsc portb,rb1    ;RB1(stop key) = ON ?
      goto check1       ;No. Next
      clrf mode          ;Yes. Set stop mode
      goto drive          ;No. Jump to motor drive
check1
      btfsc portb,rb2    ;RB2(right key) = ON ?
      goto check2       ;No. Next
      movlw d'1'          ;Yes. Set right mode
      movwf mode          ;Save mode
      goto drive          ;No. Jump to motor drive
check2
      btfsc portb,rb0    ;RB0(left key) = ON ?
      goto drive          ;No. Jump to motor drive
      movlw d'2'          ;Yes. Set left mode
      movwf mode          ;Save mode

;********************  Motor drive  *********************
drive
      movf mode,w       ;Read mode
      bz    start          ;mode = stop
      bsf    portb,rb7    ;Set RB7 = 1
      btfsc portb,rb5    ;RB5 = 0 ?
      goto $-1          ;No. Wait
      movlw d'5'          ;Set loop count(5msec)
      movwf count1       ;Save loop count
loop call timer          ;Wait 1msec
      decfsz  count1,f       ;count - 1 = 0 ?
      goto loop          ;No. Continue
      bcf    portb,rb7    ;Set RB7 = 0
      btfss portb,rb5    ;RB5 = 1 ?
      goto $-1          ;No. Wait
      movf porta,w       ;Read PORTA
      sublw b'000000101' ;Check motor position
      bnz    drive2       ;Unmatch
      movf mode,w       ;Read mode
      sublw d'1'          ;Right ?
      bz    drive1       ;Yes. Right
      movlw b'00001001'    ;No. Set Left data
      goto drive_end    ;Jump to PORTA write
drive1
      movlw b'00000110'    ;Set Right data
      goto drive_end    ;Jump to PORTA write
;-------
drive2
      movf porta,w       ;Read PORTA
      sublw b'000000110' ;Check motor position
      bnz    drive4       ;Unmatch
      movf mode,w       ;Read mode
      sublw d'1'          ;Right ?
      bz    drive3       ;Yes. Right
      movlw b'00000101'    ;No. Set Left data
      goto drive_end    ;Jump to PORTA write
drive3
      movlw b'00001010'    ;Set Right data
      goto drive_end    ;Jump to PORTA write
;-------
drive4
      movf porta,w       ;Read PORTA
      sublw b'000001010' ;Check motor position
      bnz    drive6       ;Unmatch
      movf mode,w       ;Read mode
      sublw d'1'          ;Right ?
      bz    drive5       ;Yes. Right
      movlw b'00000110'    ;No. Set Left data
      goto drive_end    ;Jump to PORTA write
drive5
      movlw b'00001001'    ;Set Right data
      goto drive_end    ;Jump to PORTA write
;-------
drive6
      movf porta,w       ;Read PORTA
      sublw b'000001001' ;Check motor position
      bnz    drive8       ;Unmatch
      movf mode,w       ;Read mode
      sublw d'1'          ;Right ?
      bz    drive7       ;Yes. Right
      movlw b'00001010'    ;No. Set Left data
      goto drive_end    ;Jump to PORTA write
drive7
      movlw b'00000101'    ;Set Right data
      goto drive_end    ;Jump to PORTA write
;-------
drive8
      movlw b'00000101'    ;Compulsion setting

drive_end
      movwf porta          ;Write PORTA
      goto start          ;Jump to start

;*************  1msec Timer Subroutine  *****************
timer
      movlw d'200'       ;Set loop count
      movwf count2       ;Save loop count
tmlp nop                   ;Time adjust
      nop                   ;Time adjust
      decfsz  count2,f       ;count - 1 = 0 ?
      goto tmlp          ;No. Continue
      return                ;Yes. Count end

;********************************************************
;          END of Stepper Motor controller
;********************************************************

      end

http://www.interq.or.jp/japan/se-inoue/e_step7.htm

step5_1.jpg (36.35 KB, 下載次數: 494)

step5_2.jpg (39.75 KB, 下載次數: 498)

作者: uucww 時間: 2006-8-16 13:27
..............................

step2_1.gif (9.67 KB, 下載次數: 509)

step2_2.gif (12.95 KB, 下載次數: 524)

作者: uucww 時間: 2006-8-16 13:32
PIC16F84A

pic2_0.gif (2.31 KB, 下載次數: 519)

歡迎光臨 Robofun 機器人論壇 (https://www.robofun.net/forum/)