'-------------------------------------------------------------------------------
Option Explicit
'-------------------------------------------------------------------------------
'HUMMER constants
Public Const backward as byte = 5	'byork appmod pin 3 -activate with duty cycle for variable speed
public const forward as byte = 6 	'byork appmod pin 4 -activate with duty cycle for varable speed	
public const left as byte = 7		'byork appmod pin 5 -set HIGH to turn, set LOW to release
public const right as byte = 8		'byork appmod pin 6 -set HIGH to turn, set LOW to release
public const neutral as byte = 9	'byork appmod pin 7 -set LOW to use, set HIGH for autocenter of wheels
public const frir as byte = 14		'the front right IR pin 13 on the appmod
public const flir as byte = 15		'the front right IR pin 12 on the appmod
public const brir as byte = 16		'the front right IR pin 14 on the appmod
public const blir as byte = 13		'the front right IR pin 11 on the appmod
public const enable as byte = 18	'enable ir pin 16 on the appmod     
'Setting up stacks for tasks
'-------------------------------------------------------------------------------
'dim StackIRdetect(1 to 70) as byte
'dim StackDriving(1 to 100) as byte
'dim StackCornerIR(1 to 32) as byte
'-------------------------------------------------------------------------------


'-------------------------------------------------------------------------------
Public Sub Main()

call putpin(26, 1)
call putpin(25, 1)
call sleep(1.0)
call putpin(25, 0)
call sleep(1.0)
call putpin(25, 1)
call sleep(1.0)

'*******************'
'code to see turning
'turn left
'call PutPin(neutral, 0) 
'call PutPin(left, 1)
'call sleep(3.0)
'call PutPin(left, 0)
'call PutPin(neutral, 1)
'call sleep(5.0)
'turn right
'call PutPin(neutral, 0) 
'call PutPin(right, 1)
'call sleep(3.0)
'call PutPin(right, 0)
'call PutPin(neutral, 1)
'call sleep(5.0)
'turn left and then right
'call PutPin(neutral, 0) 
'call PutPin(left, 1)
'call sleep(3.0)
'call PutPin(left, 0)
'call PutPin(neutral, 1)
'call PutPin(neutral, 0) 
'call PutPin(right, 1)
'call sleep(3.0)
'call PutPin(right, 0) 
'call PutPin(neutral, 1)
'call sleep(5.0)

'**************************************'
'code for moving forward then backwards
'call putpin(forward, 1)
'call sleep(0.5)
'call putpin(forward, 0)
'call sleep(5.0)
'call putpin(backward, 1)
'call sleep(0.5)
'call putpin(backward, 0)

'**************************************************'
'code for moving with pulses half a square on floor
'dim i as integer
'for i = 1 to 58
'	call putpin(forward, 1)
'	call sleep(0.001)
'	call putpin(forward, 0)
'	call sleep(0.001)
'next

'*****************************'
'code to turn 90 degrees right
'turn radius in squares on floor from a stop
'4 squares forward and 3.5 squares in direction turning
'call PutPin(neutral, 0) 
'call PutPin(right, 1)
'dim i as integer
'for i = 1 to 225
'	call putpin(forward, 1)
'	call sleep(0.001)
'	call putpin(forward, 0)
'	call sleep(0.01)
'next
'call sleep(5.0)
'call PutPin(right, 0)
'call PutPin(neutral, 1)

'****************************'
'code to turn 90 degrees left
'turn radius in squares on floor from a stop
'5.5 squares forward and 5 squares in direction turning
'call PutPin(neutral, 0) 
'call PutPin(left, 1)
'dim i as integer
'for i = 1 to 272
'	call putpin(forward, 1)
'	call sleep(0.001)
'	call putpin(forward, 0)
'	call sleep(0.01)
'next
'call sleep(5.0)
'call PutPin(left, 0)
'call PutPin(neutral, 1)

'*********************'
'Code for slowing down
'forward 116 and backward 50 - worked good at stopping
'forward 200 and backward 50 - worked good at stopping
'forward 300 and backward 60 - worked good at stopping
'forward 400 and backward 80 - worked good at stopping
'forward 500 and backward 90 - worked good at stopping
'forward 600 and backward 90 - worked good at stopping
'I'm not sure but I don't think the Hummer is accelerating anymore
'Need to test longer forward intervals
'dim i as integer
'for i = 1 to 900  
'	call putpin(forward, 1)
'	call sleep(0.001)
'	call putpin(forward, 0)
'	call sleep(0.01)
'next
'stop code
'for i = 1 to 90
'	call putpin(backward, 1)
'	call sleep(0.001)
'	call putpin(backward, 0)
'	call sleep(0.01)
'next

'**************'
'code for sound


'**************************'
'Code to go to Dr. J's Door

'forward 45 squares and turn left

call putpin(neutral, 1)
dim i as integer
for i = 1 to 925
	call putpin(forward, 1)
	call sleep(0.001)
	call putpin(forward, 0)
	call sleep(0.01)
next
'stop code
for i = 1 to 100
	call putpin(backward, 1)
	call sleep(0.001)
	call putpin(backward, 0)
	call sleep(0.01)
next
call sleep(0.5)
call PutPin(neutral, 0) 
call PutPin(left, 1)

for i = 1 to 260
	call putpin(forward, 1)
	call sleep(0.001)
	call putpin(forward, 0)
	call sleep(0.01)
next
call sleep(2.5)
call PutPin(left, 0)
call PutPin(neutral, 1)

call sleep(0.5)

for i = 1 to 1600
	call putpin(forward, 1)
	call sleep(0.001)
	call putpin(forward, 0)
	call sleep(0.01)
next
'stop code
for i = 1 to 100
	call putpin(backward, 1)
	call sleep(0.001)
	call putpin(backward, 0)
	call sleep(0.01)
next







'dim i as integer
'for i = 1 to 10
'	call pulseout(forward, 60000, 1)
'	call pulseout(forward, 922, 0)
	'call speedcontrol(forward, 2500, 1000)
'	call sleep(0.5)
'	call pulseout(25,10000,1)
'	call pulseout(26,10000,1)
	
'	call pulseout(26,6000,0)
'	call sleep(0.5)

'	call putpin(26, 0)
'next


'dim idx as byte


'CallTask "irdetect", 	StackIRdetect
'CallTask "driving", 	StackDriving
'CallTask "cornerIR", 	StackCornerIR

'Do
	'call sleep(400.0)

	'call irdetect()
'	call cornerir()
'Loop
'Do
'call speedcontrol(forward, 30.0, 70.0)  '50% HIGH duty with 20ms period,  50hz	|
'call driving()
'Loop


'call turncontrol(neutral, right)	'turn OFF autocenter and turn ON right steer
'call turncontrol(right, neutral)	'turn OFF right steer and SET autocenter

	'call irdetect()
	'Select Case (DataByte)
		'Case 0
			'no objects detected continue driving forward
		'Case 2
			'object detected on left, turn right and avoid 
		'Case 4
			'object detected on right, turn left and avoid
		'Case 6
			'object on left and right, backup and turn left or right	
	'End Select 



'Debug.Print "End of loop."
End Sub
'-------------------------------------------------------------------------------