DEFINE ONINT_USED 1	'This line allows the use of a bootloader

'For PIC16F876
'This program is made to accept serial i/o from the hardware port,
'and relay it to a servo controller via 2 serial pins, using flow
'control.  The other chip is a PIC16F84, program name 84srv2.
'
steps		CON 20		'The number of steps to take when changing height
i			VAR BYTE 	'index variable
j			VAR BYTE	'index variable
offset		VAR BYTE	'Offset mount for balance routine
NewHR		VAR BYTE	'Height after adding offset (Right leg)
NewHL		VAR BYTE	'Height after adding offset (Left leg)
height		VAR BYTE	'Standing height, 0-100, 0=sit, 100=stand
RAO			VAR BYTE	'Right Ankle Angle	'Ankle offset to balance in X dir
LAO			VAR BYTE	'Left Ankle Angle	'Ankle offset to balance in Y dir
RHO			VAR BYTE	'Right Height Offset
LHO			VAR BYTE	'Left Height Offset
RKO			VAR BYTE	'Right knee offset
LKO			VAR BYTE	'Left knee offset
svsit		VAR BYTE[7]	'Servo settings for sitting position
svstand		VAR BYTE[7]	'Servo settings for standing position
svset 		VAR BYTE[7]	'Servo setpoint
NumServos 	CON 6		'Number of last servo connected
hTXpin		VAR PORTC.6	'hardware serial TX pin
hRXpin		VAR PORTC.7	'hardware serial RX pin
htimeout	CON 2		'hardware timeout parm
cr			CON 13		'carriage return ASCII val
lf			CON 10		'linefeed ASCII val
svRXpin		VAR PORTA.4	'pin to receive serial data
svTXpin		VAR PORTC.0	'pin to transmit serial data
svflowpin	VAR svrxpin	'use the rx pin for flow control
svtimeout	CON 30		'timeout for serial out
inbuffer	VAR BYTE[40]	'incoming serial buffer
svsermode	CON 84		'Serial mode setting for 9600,8,n,1
hsermode	CON 396
 
'Acceleration parameters
XApin 	VAR PORTC.2 'X acceleration
YApin 	VAR PORTC.1 'Y acceleration
XA 		VAR WORD
YA 		VAR WORD
xaoff 	VAR WORD
yaoff	VAR xaoff	'Share same var space with xaoff
axzero 	CON 52
ayzero 	CON 48

RAO=0:LAO=0:RHO=0:LHO=0:RKO=0:LKO=0	'Set all balance offsets to zero

'Set Sit and Stand constants to define sit,stand positions
svsit[1]=35:svsit[2]=0:svsit[3]=20
svsit[4]=225:svsit[5]=205:svsit[6]=215
svstand[1]=160:svstand[2]=210:svstand[3]=130
svstand[4]=90:svstand[5]=0:svstand[6]=95

DEFINE HSER_BAUD 4800	'Set hardware serial port baud rate

'PAUSE 5000		'THIS IS TEMPORARY - JUST TO MAKE IT AUTOMATIC
'GOTO RUNSCAN		'AND START MOVING BY ITSELF

MAIN:
GoSub playdead
Pause 3000
GoSub sitdown
GoSub wakeup
Pause 1000
For HEIGHT = 0 TO 180 STEP 3
GoSub SETHEIGHT
Next HEIGHT
GoTo BALANCE

'Receive serial data, with a timeout value, and assign the data to 'inbyte array
j=0 
serloop:
HSerin htimeout,skipto,[inbuffer[j]]
HSerout [inbuffer[j]]	'echo chars to screen
IF inbuffer[j] = 13 Then passiton
j=j+1
skipto:
GoTo serloop

passiton:
'Send contents of inbuffer array to PIC16F84 (servo controller)
IF inbuffer[0]= cr Then sendprompt
IF inbuffer[0]="R" Then runscan
IF inbuffer[0]="B" Then balance
IF inbuffer[0]="V" Then sendversion
SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,[STR inbuffer\j]
GoTo sendprompt
timerror:
HSerout [cr,lf,"T!"]
GoTo sendprompt

sendprompt:
HSerout [cr,lf,">"]
j=0
GoTo serloop

sendversion:
HSerout [cr,lf,"76srv2"]
GoTo sendprompt

SetServos:
For i=6 to 1 step -1
SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
["S",DEC1 i,DEC3 svset[i]]
'HSerout ["S",DEC1 i," ",DEC3 svset[i]," "]
Next i
'HSerout [" ",DEC3 RAO," ",DEC3 LAO," ",DEC3 RHO," ",DEC3 LHO,"h ",DEC HEIGHT,cr,LF]
'Pause 500
Return

WakeUp:
SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
["E9"]		'Enable all servos
Return

PlayDead:
SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
["E0"]		'Disable all servos
Return

SitDown:
For i=1 to 6
svset[i]=svsit[i]
Next i
GoSub setservos
Return

SetHeight:
'Set Right Height
NewHR = height + RHO
svset[1]=svsit[1]+(NewHR)*(125)/255
svset[2]=svsit[2]+(NewHR)*(230)/255
svset[3]=svsit[3]+(NewHR)*(110)/255
NewHL = height + LHO
svset[4]=svsit[4]-(NewHL)*(155)/255
svset[5]=svsit[5]-(NewHL)*(205)/255
svset[6]=svsit[6]-(NewHL)*(120)/255

svset[1]=svset[1]+RAO	'Compensate for ankle offsets
svset[4]=svset[4]+LAO
svset[2]=svset[2]+RKO	'Compensate for knee offsets
svset[5]=svset[5]+LKO
GoSub setservos
Return

Balance:
GoSub SetHeight
SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
["E9"]		'Enable all servos
Pause 1000
HSerout [CR,LF]
GoSub getaccel	'get accel readings from accelerometer

'Now start active control to maintain this level setting
Ctrloop:
GoSub SetHeight
GoSub getaccel

IF xa = axzero Then CheckY
offset = (ABS (xa-axzero)) MIN 3	'The greater the tilt angle the greater the offset to compensate
IF xa > axzero Then gofwd
goback:
RKO = RKO + 1
LKO = LKO - 1
RAO = RAO - offset
LAO = LAO + offset
GoTo CheckY
gofwd:
RKO = RKO - 1
LKO = LKO + 1
RAO = RAO + offset
LAO = LAO - offset
CheckY:
IF ya = ayzero Then ctrloop
offset = (ABS (ya-ayzero)) MIN 5
IF ya > ayzero Then LeanRight
LeanLeft:
RHO = RHO - offset
LHO = LHO + offset
GoTo ctrloop
LeanRight:
RHO = RHO + offset
LHO = LHO - offset
GoTo ctrloop

getaccel:
PulsIn xapin,1,xa
PulsIn xapin,0,xaoff
xa = 100*xa/(xaoff+xa)
PulsIn yapin,1,ya
PulsIn yapin,0,yaoff
ya = 100*ya/(xaoff+ya)
'HSerout ["X = ",DEC xa,"%","   Y = ",DEC ya,"%",13,10]
Return


runscan:
'hserout [CR,LF,"Standing..."]
'Enable all servos
'SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
'["E9", CR, LF]
'Pause 100
'For j = 1 to steps
'svset[1]=35 + ((180-35)*j)/steps
'svset[2]=0 + ((230-0)*j)/steps
'svset[3]=20 + ((130-20)*j)/steps
'svset[4]=225 - ((225-70)*j)/steps
'svset[5]=205 - ((205-0)*j)/steps
'svset[6]=215 - ((215-95)*j)/steps
'For i = 1 to 6
'SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
'["S",DEC1 i,DEC3 svset[i]," "]
''Hserout ["S",DEC1 i,DEC3 svset[i]," "]
'Next i
'Pause 2
'Next j
'HSerout ["Done"]
'Pause 1500
'For i = 1 to 10
'svset[3] =130+6*i
'svset[6] = 95-6*i
'SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
'["S3",DEC3 svset[3]," S6",DEC3 svset[6],cr]
'Pause 10
'Next i
'Pause 500
'For i = 10 to 1 step -1
'svset[3] = 130+6*i
'svset[6] = 95-6*i
'SerOut2 svtxpin\svflowpin,svsermode,svtimeout,timerror,_
'["S3",DEC3 svset[3]," S6",DEC3 svset[6],cr]
'Pause 6
'Next i
'GoTo sendprompt