'***********************************************************
'**                                                       **
'**            Simple Program for Lynxmotion              **
'**      EH2 Hexapod using a BX24 Microcontroller         **
'**           and a SD21 Devantech Servo Controller       **
'**	      the SD21 is connected to a 3 channel        **
'**		    R/C receiver                          **
'**					                  **
'**			TESTED                            **
'**                    					  **
'**            Copyright 2004 - Luigi Carnevale           **
'**     Commercial use of this software is prohibited     **
'**     Private and educational use only is permitted     **
'**                                                       **
'**                                                       **
'***********************************************************

Option Explicit

Const SCL As Byte = 6                  ' I2C clock - SCL
Const SDA As Byte = 5                  ' I2C data - SDA

Const SD21 As Byte = &HC2		'SD21 I2C Address

Dim Speed As Byte					
Dim Height As Byte

Const FwdCh As Byte = 12			'Forward/Backward Stick (CH2)
Const LRCh As Byte = 7				'Left/Right Stick (CH4)
Const HeightCh As Byte = 8			'Height Linear Pot  (CH5)

Dim FwdPulse As Integer				'Forward/Backward Pulse (CH2)
Dim LRPulse As Integer				'Left/Right Pulse (CH4)
Dim HeightPulse As Integer			'Height Pulse  (CH5)

'--------------------------------
'Tune the servo max range here - important for robot safety -
'--------------------------------

Const MaxSpeed As Byte = 35

Const MaxHeight As Byte = 40


'--------------------------------
'Servo Configuration
'--------------------------------

'Left Front Propulsion -> Servo 17

Const LFPcentre As Byte = 79	'Left Front Propulsion Centre value
Const LFPbck As Byte = 100		'Left Front Propulsion Back value
Const LFPfwd As Byte = 121		'Left Front Propulsion Forward value

'Left Front Lift -> Servo 18

Const LFLcentre As Byte = 80	'Left Front Lift Centre value
Const LFLdwn As Byte = 101		'Left Front Lift Down value
Const LFLup As Byte = 122		'Left Front Lift Up value


'Right Front Propulsion -> Servo 15

Const RFPcentre As Byte = 77
Const RFPfwd As Byte = 98
Const RFPbck As Byte = 119

'Right Front Lift -> Servo 16

Const RFLcentre As Byte = 78
Const RFLup As Byte = 99
Const RFLdwn As Byte = 120

'Left Middle Propulsion -> Servo 7

Const LMPcentre As Byte = 69
Const LMPfwd As Byte = 90
Const LMPbck As Byte = 111

'Left Middle Lift -> Servo 8

Const LMLcentre As Byte = 70
Const LMLdwn As Byte = 91
Const LMLup As Byte = 112

'Right Middle Propulsion -> Servo 9

Const RMPcentre As Byte = 71
Const RMPbck As Byte = 92
Const RMPfwd As Byte = 113

'Right Middle Lift -> Servo 10

Const RMLcentre As Byte = 72
Const RMLup As Byte = 93
Const RMLdwn As Byte = 114

'Left Rear Propulsion -> Servo 11

Const LRPcentre As Byte = 73
Const LRPbck As Byte = 94
Const LRPfwd As Byte = 115

'Left Rear Lift -> Servo 12

Const LRLcentre As Byte = 74
Const LRLdwn As Byte = 95
Const LRLup As Byte = 116

'Right Rear Propulsion -> Servo 13

Const RRPcentre As Byte = 75
Const RRPfwd As Byte = 96
Const RRPbck As Byte = 117

'Right Rear Lift -> Servo 14

Const RRLcentre As Byte = 76
Const RRLup As Byte = 97
Const RRLdwn As Byte = 118

'--------------------------------
'Servo Configuration End
'--------------------------------




Sub Main()

    	Call PutPin(SCL, bxOutputHigh)
	Call PutPin(SDA, bxOutputHigh)

	Speed = 10
	


'Centering Servo

    	Call I2cByteWrite(SD21, LFLcentre, 128)
	Call I2cByteWrite(SD21, LFPcentre, 128)
	Call I2cByteWrite(SD21, RFLcentre, 128)
	Call I2cByteWrite(SD21, RFPcentre, 128)
	Call I2cByteWrite(SD21, LMLcentre, 128)
	Call I2cByteWrite(SD21, LMPcentre, 128)
	Call I2cByteWrite(SD21, RMLcentre, 128)
	Call I2cByteWrite(SD21, RMPcentre, 128)
	Call I2cByteWrite(SD21, LRLcentre, 128)
	Call I2cByteWrite(SD21, LRPcentre, 128)
	Call I2cByteWrite(SD21, RRLcentre, 128)
	Call I2cByteWrite(SD21, RRPcentre, 128)
	Delay(0.3)

'----------Program Start---------------------

Do

'Channel scanning----------------------------

	FwdPulse = PulseIn(FwdCh, 1)
	'Debug.Print "FwdCh = " ; Cstr(FwdPulse)			'Useful for checking incoming 
	'Delay(0.2)									'signal from receiver

	LRPulse = PulseIn(LRCh, 1)
	'Debug.Print "LRCh = " ; Cstr(LRPulse)
	'Delay(0.2)

	HeightPulse = PulseIn(HeightCh, 1)
	'Debug.Print "HeightCh = " ; Cstr(HeightPulse)
	'Delay(0.2)

'Channel scanning End------------------------

'Decoding Commands---------------------------

	Dim NoCmd As Boolean					'This is a Flag useful for auto-centering function
	Dim BotStopped As Boolean				'This Flag tells StopBot Sub the Robot is already stopped

	FwdPulse = (FwdPulse - 650) \ 6
	
	LRPulse = (LRPulse -650) \ 6

	HeightPulse = (HeightPulse - 650) \ 6

	NoCmd = True
	

	If FwdPulse < 120 then
		Speed = CByte(120 - FwdPulse)
		if Speed > MaxSpeed then
			Speed = MaxSpeed
		End if
		Call WalkFwd
		NoCmd = False
		BotStopped = False
	End if

	If FwdPulse > 130 then
		Speed = CByte(FwdPulse - 130)
		if Speed > MaxSpeed then
			Speed = MaxSpeed
		End if
		Call WalkBack
		NoCmd = False
		BotStopped = False
	End if

	If LRPulse < 120 then
		Speed = CByte(120 - LRPulse)
		if Speed > MaxSpeed then
			Speed = MaxSpeed
		End if
		Call TurnLeft
		NoCmd = False
		BotStopped = False
	End if

	If LRPulse >130 then
		Speed = CByte(LRPulse - 130)
		if Speed > MaxSpeed then
			Speed = MaxSpeed
		End if
		Call TurnRight
		NoCmd = False
		BotStopped = False
	End if

	Height = MaxHeight

	If HeightPulse < 120 then
		Height = CByte(120 - HeightPulse)
		If Height > MaxHeight then
			Height = MaxHeight
		End if
	Elseif HeightPulse >130 then
		Height = CByte(HeightPulse - 130)
		If Height > MaxHeight then
			Height = MaxHeight
		End if
	End if

	If (NoCmd = True) and (BotStopped = False) then
		Call StopBot
		BotStopped = True
	End if

	
'Decoding Commands End-----------------------

Loop

'----------Program End-----------------------

End Sub

'--------------------------------
'Sub Routines
'--------------------------------


Sub WalkFwd()
	Call LiftLeft
	Delay(0.1)
	Call BackRight
	Delay(0.1)
	Call FwdLeft
	Delay(0.1)
	Call LowLeft
	Delay(0.1)
	Call LiftRight
	Delay(0.1)
	Call BackLeft
	Delay(0.1)
	Call FwdRight
	Delay(0.1)
	Call LowRight
	Delay(0.1)
End Sub

Sub WalkBack()
	Call LiftLeft
	Delay(0.1)
	Call FwdRight
	Delay(0.1)
	Call BackLeft
	Delay(0.1)
	Call LowLeft
	Delay(0.1)
	Call LiftRight
	Delay(0.1)
	Call FwdLeft
	Delay(0.1)
	Call BackRight
	Delay(0.1)
	Call LowRight
	Delay(0.1)
End Sub

Sub TurnLeft()
	Call LiftLeft
	Delay(0.1)
	Call BackRightRev
	Delay(0.1)
	Call BackLeftRev
	Delay(0.1)
	Call LowLeft
	Delay(0.1)
	Call LiftRight
	Delay(0.1)
	Call FwdLeftRev
	Delay(0.1)
	Call FwdRightRev
	Delay(0.1)
	Call LowRight
	Delay(0.1)
End Sub

Sub TurnRight()
	Call LiftRight
	Delay(0.1)
	Call BackLeftRev
	Delay(0.1)
	Call BackRightRev
	Delay(0.1)
	Call LowRight
	Delay(0.1)
	Call LiftLeft
	Delay(0.1)
	Call FwdRightRev
	Delay(0.1)
	Call FwdLeftRev
	Delay(0.1)
	Call LowLeft
	Delay(0.1)
End Sub

'This sub reset the starting position

Sub StopBot()
	Call LowLeft
	Call LowRight
	Delay(0.1)
	Call LiftLeft
	Delay(0.1)
	Call CentreLeft
	Delay(0.1)
	Call LowLeft
	Delay(0.1)
	Call LiftRight
	Delay(0.1)
	Call CentreRight
	Delay(0.1)
	Call LowRight
	Delay(0.1)
End Sub

Sub LiftLeft()
	Call I2cByteWrite(SD21, LFLup, Height)
	Call I2cByteWrite(SD21, RMLup, Height)
	Call I2cByteWrite(SD21, LRLup, Height)
End Sub

Sub LiftRight()
	Call I2cByteWrite(SD21, RFLup, Height)
	Call I2cByteWrite(SD21, LMLup, Height)
	Call I2cByteWrite(SD21, RRLup, Height)
End Sub

Sub LowLeft()
	Call I2cByteWrite(SD21, LFLdwn, Height)
	Call I2cByteWrite(SD21, RMLdwn, Height)
	Call I2cByteWrite(SD21, LRLdwn, Height)
End Sub

Sub LowRight()
	Call I2cByteWrite(SD21, RFLdwn, Height)
	Call I2cByteWrite(SD21, LMLdwn, Height)
	Call I2cByteWrite(SD21, RRLdwn, Height)
End Sub

Sub FwdLeft()
	Call I2cByteWrite(SD21, LFPfwd, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, RMPbck, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, LRPfwd, Speed)
End Sub

Sub FwdRight()
	Call I2cByteWrite(SD21, RFPfwd, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, LMPbck, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, RRPfwd, Speed)
End Sub

Sub BackLeft()
	Call I2cByteWrite(SD21, LFPbck, Speed)
	Call I2cByteWrite(SD21, RMPfwd, Speed)
	Call I2cByteWrite(SD21, LRPbck, Speed)
End Sub

Sub BackRight()
	Call I2cByteWrite(SD21, RFPbck, Speed)
	Call I2cByteWrite(SD21, LMPfwd, Speed)
	Call I2cByteWrite(SD21, RRPbck, Speed)
End Sub

Sub CentreLeft()
	Call I2cByteWrite(SD21, LFPcentre, 128)
	Call I2cByteWrite(SD21, RMPcentre, 128)
	Call I2cByteWrite(SD21, LRPcentre, 128)
End Sub

Sub CentreRight()
	Call I2cByteWrite(SD21, RFPcentre, 128)
	Call I2cByteWrite(SD21, LMPcentre, 128)
	Call I2cByteWrite(SD21, RRPcentre, 128)
End Sub

Sub FwdLeftRev()
	Call I2cByteWrite(SD21, LFPfwd, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, RMPfwd, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, LRPfwd, Speed)
End Sub

Sub FwdRightRev()
	Call I2cByteWrite(SD21, RFPfwd, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, LMPfwd, Speed)
	Delay(0.1)
	Call I2cByteWrite(SD21, RRPfwd, Speed)
End Sub

Sub BackLeftRev()
	Call I2cByteWrite(SD21, LFPbck, Speed)
	Call I2cByteWrite(SD21, RMPbck, Speed)
	Call I2cByteWrite(SD21, LRPbck, Speed)
End Sub

Sub BackRightRev()
	Call I2cByteWrite(SD21, RFPbck, Speed)
	Call I2cByteWrite(SD21, LMPbck, Speed)
	Call I2cByteWrite(SD21, RRPbck, Speed)
End Sub

'--------------------------------
'I2C Sub Routines by Gerald Coe
'--------------------------------

' writes I2cData to I2cReg at I2cAddr

Sub I2cByteWrite(ByVal I2cAddr As Byte, ByVal I2cReg As Byte, ByVal I2cData As Byte)
    Call I2cStart()
    Call I2cOutByte(I2cAddr)                   ' send device address
    Call I2cOutByte(I2cReg)                    ' send register address
    Call I2cOutByte(I2cData)                   ' send the data
    Call I2cStop()
End Sub

Sub I2cOutByte(I2cData As Byte)
    Call ShiftOut(SDA, SCL, 8, I2cData)        ' shift data out
    Call PutPin(SDA, bxInputTristate)          ' turn SDA around
    Call PutPin(SCL, bxOutputHigh)             ' and clock in the ack' bit
    Call PutPin(SCL, bxOutputLow)
End Sub

Sub I2cStart()                                 ' I2C start bit sequence
    Call PutPin(SDA, bxOutputHigh)
    Call PutPin(SCL, bxOutputHigh)
    Call PutPin(SDA, bxOutputLow)
    Call PutPin(SCL, bxOutputLow)
End Sub

Sub I2cStop()                                  ' I2C stop bit sequence
    Call PutPin(SDA, bxOutputLow)
    Call PutPin(SCL, bxOutputHigh)
    Call PutPin(SDA, bxOutputHigh)
End Sub