Back to this project.
Imagine a sewing machine. When the needle goes down, the thread is held steady and travels with the needle. When the needle goes up, the thread is fed out at the same rate that it travels up, feeding slack. There is a servo motor with an encoder feeding the thread and a matching encoder reading the travel distance and speed. There is also an aux encoder (ManDrive) that can be used to retract or feed the thread to trim or allow for excess so you can cut the thread. The manual feed/trim works as expected, but after a manual encoder change, sometimes the travel feed operates on both directions of input from the travel encoder instead of just retract. The feed is the correct direction, but when bad, the "down" feeds at twice the input rate while the "up" feeds normally. A change on the Man encoder allows it to work correctly again.
Code:
' 1) Quadrature encoder reading direction and speed of mechanical slide.
' 2) Motor (servo) driven in ONE direction of slide movement,
' not driven for other direction. Pulse & Direction drive LOW.
' 3) Manual Encoder to drive servo either direction manually.
' ADD pull-up to GPIO.3 on 12F675
' DEBUG splash on power-up on GPIO.5
' 12F675
' -----------u----------
' -|Vdd(+) (-)Vss |-
' D_out -|GP5 GP0/PGD|- A_trav
' P_out -|GP4 GP1/PGC|- B_trav
' B_man -|GP3/MCLR/Vpp GP2/INT|- A_man
' ----------------------
DEFINE DEBUG_REG GPIO
DEFINE DEBUG_BIT 5
DEFINE DEBUG_BAUD 2400 '
DEFINE DEBUG_MODE 0
DEFINE OSC 4
clear
A_trav VAR GPIO.0 ' Travel encoder A input
B_trav VAR GPIO.1 ' Travel encoder B input
A_man var GPIO.2 ' Manual Encoder A input
B_man var GPIO.3 ' Manual Encoder B input
P_out var GPIO.4 ' PULSE output to motor, Active LOW
D_out var GPIO.5 ' LEVEL output for direction
AutoCnt var word 'preprogrammed feed @ end of cycle
Dir VAR byte 'Direction
EncOld var byte
EncNew var byte
ManCnt var byte 'Counter for recent Man Pulses
TrvCnt var byte ' Travel Encoder before change
INTCON = %10001000 ' GLOBAL ON, GPIO Int ON
CMCON = 7 ' Comparator off, Digital I/O
OPTION_REG = %00000000 ' GPIO Pull-up Enabled
TRISIO = %001111
WPU = %11111111 ' weak p/u on 0-2,4,5, GPPU must be enabled for p/u
IOC = %00001111 ' Interrupt on Change for GPIO: 0-3
ANSEL = 0 ' Analog off
ManCnt = 0
TrvCnt = 0
P_out = 1 ' start out HI. Moves on LOW pulse
INCLUDE "DT_INTS-14-0.bas" 'Base Inturrupt program:0 banks
'include "ReEnterPBP-0.pbp" 'allow PBP int
ASM
RBIF = GPIF
RBIE = GPIE
INT_LIST macro ;IntSource, Label, Type, ResetFlag?
INT_Handler RBC_INT, _EncChg, ASM, yes
endm
INT_CREATE ;creates the INT processor
INT_ENABLE RBC_INT ;enable RB change int
ENDASM
debug "LW-FiberDrive-7"
Main:
ManualDrive: ' MANUAL first: set direction bit
if ManCnt > 0 then
Dir = EncNew.3 ^ EncOld.2 ' XOR new L bit > Old Rt bit=Direction
D_out = Dir 'set direction pin
pauseus 2
P_out = 0 'send drive pulse either dir
pauseus 2
P_out = 1 'reset drive pulse
ManCnt = ManCnt - 1 ' empty out pulse counter
endif
D_out = 1 'set direction pin
TravelDrive:
D_out = 1 'set direction pin
if TrvCnt > 0 then
Dir = EncNew.1 ^ EncOld.0 ' XOR new L bit > Old Rt bit=Direction
If DIR = 0 then TravDown 'end if travel DOWN
pauseus 2
P_out = 0 'send drive pulse retract only
pauseus 2
P_out = 1 'reset drive pulse
TravDown:
TrvCnt = TrvCnt - 1
endif
goto main
'++++++++ Motor Drive Pulse Out interrupt handler ++++++++++++++++++
EncChg: 'got here if GPIO 0-3 changed
EncOld = EncNew
EncNew = GPIO
if (EncNew & (%00001100)) = (EncOld & (%00001100))then ' man enc same?
TrvCnt = TrvCnt + 1 ' trav moved: count travel interrupts
endif
ManCnt = ManCnt + 1 ' else: Man moved:count man interrupts
@ INT_RETURN
end
Anyone care to take another look?
Progress, but not right yet...
Originally Posted by boroko
Bookmarks