BrainStrain
Member
Program Code Post
End words of test Post
Code:
'****************************************************************
'* Name : ServoLock *
'* Author : Tarkan Akdam *
'* Notice : Copyright (c) 2012 Tarkan Akdam *
'* : All Rights Reserved *
'* Date : 20/06/2012 *
'* Version : 1.6 *
'* Notes : 12F629 *
'* : This circuit moves a standard RC servo between *
'* : two programmed and stored positions *
'* : Each position is selected by logic level on GP3 *
'* : The Servo is disabled after 5 seconds *
'* : which is plenty of time for the servo to reach *
'* : the final positions requested. *
'****************************************************************
'* :Circuit Pin Configuration *
'* :GP3- ALARM IN - High is Position Two *
'* :GP0- Move Servo Up *
'* :GP1- Program Mode *
'* :GP2- Move Servo Down *
'* :GP5- Servo Pulse Out *
'* :GP4- HeartBeat LED *
'****************************************************************
define OSC 4
DEFINE OSCCAL_1K 1
CMCON = 7 'comparator off
WPU = %00110111 ' Internal pull-ups = on
OPTION_REG = %00001000 ' Pull-ups = yes, GPIO.2 = I/O, prescaler to WDT
INTCON = %00000000 ' disable all interupts
GPIO = %00000000 ' All outputs = 0 on boot
TRISIO = %00001111 ' GP4 and GP5 output
IOC = %00000000 'disable IOC interrupt
'**********************
'Constants
MaxPos CON 262 'hard limits of servo movement - as measured for my servos
MinPos con 68
'**********************
'Variablles
ServoProgram var gpio.1
ServoUp VAR gpio.0
ServoDown var gpio.2
ServoPulse var gpio.5
LEDOut var gpio.4
AlarmIn var gpio.3
HeartBeat var byte
TimeOut var byte
Counter var byte
DataSaved var byte
PosTWO var word 'stores the PosTWO position
PosONE var word 'stores the PosONE position
ServoPosition var word 'current position of servo
PrevPosition var word 'old position of servo
'*****************************************
'read stored positions from eeprom before proceeding
read 00, word PosTWO 'PosTWO is stored addr 0,1
READ 02, word PosONE 'PosONE is stored at addr 2,3
'*****************************************
'make sure outputs and variables are how we want them
ServoPulse = 0 'servo signal line to 0v
HeartBeat = 0
TimeOut = 0
DataSaved = 0
' main program loop
main:
IF HeartBeat = 35 then 'flash heartbeat led
HeartBeat = 0
Toggle LEDOut 'toggle led
else
HeartBeat = HeartBeat + 1
endif
'check if we are in program mode
if ServoProgram = 0 then gosub ProgrammingMode
'are we PosTWO or PosONE
if AlarmIn = 1 then
ServoPosition = PosTWO
else
ServoPosition = PosONE
endif
gosub ServoMove
goto main
'************************************
'* This sub moves servo to programmed position
ServoMove:
IF ServoPosition = PrevPosition then 'is servo position the same as previous
IF TimeOut < 250 then TimeOut = TimeOut + 1 'then start TimeOut Counter
else
TimeOut = 0 'else reset timeout and save current position
DataSaved = 0 'reset eeprom flag
PrevPosition = ServoPosition
endif
IF TimeOut = 250 then goto skippulse 'if timed out skip moving servo
ServoPulse = 0 '0v signal - put signal line in to a known state before pulseout
pulsout ServoPulse, ServoPosition
skippulse: 'jump here to not bother with pulse
pause 19 'wait 19ms
return 'go back
'****************************************
'* Programming Mode
ProgrammingMode:
LEDOut = 0 'Turn ON LED to show program mode
IF AlarmIn = 0 then
IF ServoUp = 1 and ServoDown = 0 ThEN PosONE = PosONE + 1
IF ServoUp = 0 and ServoDown = 1 ThEN PosONE = PosONE - 1
else
IF ServoUp = 1 and ServoDown = 0 ThEN PosTWO = PosTWO + 1
IF ServoUp = 0 and ServoDown = 1 ThEN PosTWO = PosTWO - 1
endif
IF PosTWO > MaxPos then PosTWO = MaxPos 'lets make sure position is not outside
IF PosTWO < MinPos then PosTWO = MinPos 'these defined hard limits
IF PosONE > MaxPos then PosONE = MaxPos
IF PosONE < MinPos then PosONE = MinPos
IF TimeOut > 249 then 'store values in to eeprom if we have reached timeout limit
for Counter = 0 to 5 'Quick Flash LED - let user know we have saved the values
toggle LedOut
pause 100
next Counter
IF DataSaved = 0 then 'only write to eeprom if datasaved flag is 0
DataSaved = 1 'set flag to stop eeprom writes until position has changed
write 00, word PosTWO 'write the data to the eeprom
write 02, word PosONE
endif
endif
return 'go back
end