' Program för BOE-BOT / GROWBOT MED IROD, IR-Detektor
' Hårdvaran beskrivs på http://www.robotshop.nu
' C MUX-Elektronik, H. Forsberg 00-01-30 info@robotshop.nu

'Programmet kör roboten framåt, samtidigt som IROD-Modulen, monterad
'på ett R/C-servo sveper höger-vänster och detekterar hinder.
'Sveprörelsen styrs av en indexerad lookup datasats
'Samma index används för att "slå upp" ett "väjningsvärde" i 
'en korresponderande data sats. Genom att förändra dessa värden kan
'du förändra karaktären på undanmanöver...

'Programmet har också en "ta sig loss" funktion som utlöses när mcount  
'når ett visst läge, varvid roboten backar i en S-rörelse och sedan tar
'en liten paus, observera sleep-functionen som minimerar strömförbrukningen
'Sammantaget har roboten mycket stor förmåga att hålla sig rllande bland 
'i ganska svår miljö.  

				'Define I/O
Radarmotor	con	11	'Motor som roterar IROD
rightWheel 	con 	13	'Höger hjul (servo)
leftWheel	con	12	'Vänster (servo)
i		var   byte  'Räknare för diverse loopar
index       var   byte  'Position Radar
speed		var	word	'Aktuell hastighet ( positiva / negativa)
steer		var	word	'Aktuell styrparameter ( positiva / negativa)
antal		var   byte  'Parameter som ingår i timingen till R/C-servona
mcount	var	byte	'Ta digloss räknare
hinder      var   byte  'Etta om IROD-rutinen detekterat hinder
radar_vinkel var  word  'Här ligger aktuell vinkel till radarmotorn
				'Define Constants
rightOff	con	750	'   "off" pulse för servo
leftOff	con	750	'   "off" pulse för servo
radaroff    con   750   '   750 är mittläge, rakt fram på radarmotor
'---------------- program start--------------------------------------
Init:	dirh = $B9			'Set I/O direction
	outh = $00			'Set outputs = OFF

start:
if mcount > 60 then backa   ' Testa om det är dags att lösa ut "ta dig loss funktionen"
mcount = mcount + 1         ' Stega upp Ta dig loss räknaren 


speed=250    ' Normalläge full fart framåt  
steer=0
antal = 4


for index = 1 to 14
Lookup index, [350,450,550,650,750,850,950,1050,1150,1050,950,850,750,650,550,450], radar_vinkel

'-----------------
for i = 0 to antal
gosub drive
next
'-----------------
' Testa om IROD hittar något hinder
gosub irod
If hinder =1 then turn ' Finns det något i vägen väjer vi!
next

goto start
turn:
Lookup index, [-50,-150,-200,-250,-300,250,200,150,100,50,100,200,300,-250,-150,-50], steer
speed = 50 

for i = 0 to 20
          pulsout  leftWheel,leftOff +speed+steer  
	    Pulsout rightWheel,rightOff-speed+steer  
		pulsout radarmotor,radar_vinkel  'sänd ut signalen till radarmotor
		pause 10
next
goto start ' Tillbas till början av programmet


'-- sänder pulser till servon  -----------
'Skickar signalerna till ett servo i taget för att "jämna ut gången OCH
'strömförbrukningen...
drive:
		pulsout  leftWheel,leftOff +speed+steer  'Left servo       = 738
'pause 5
		pulsout rightWheel,rightOff-speed+steer  'right servo null = 744
'pause 5	
      	pulsout radarmotor,radar_vinkel  'sänd ut signalen till radarmotor
'pause 5

return

'++++++++++++++++

'-- Här testas IROD detektorn, Tar ca 65 ms -----------
'IROD behöver ca 35 mS för att testa, denna paus skapar jag med lite
'körningar av höger/vänster hjul, annars stannar roboten under denna paus.
'Ganska nöjd med den lösningen!! ;-)
Irod: 
hinder = 0
high 15 'IROD startar en testsekvensgenom hög-låg på "grön" 
pause 5
low 15
'-- gör dessa motoranrop, för att skapa den pause som IROD behöver
	pulsout  leftWheel,leftOff +speed+steer  'Left servo       = 738
	pulsout rightWheel,rightOff-speed+steer  'right servo null = 744
	pause 10

	pulsout  leftWheel,leftOff +speed+steer  'Left servo       = 738
	pulsout rightWheel,rightOff-speed+steer  'right servo null = 744
	pause 10

if in14 = 0 then detekt ' Svaret från IROD kommer på "Blå"
return
detekt:
hinder = 1
low 0
pause 5
high 0
pause 5
return
'++++++++++++++++++++++++++++

'deta är "ta dig loss funktionen som hindrar roboten från att fastna
'Roboten backar i S-rörelse... avslutas med ca 10 sek SLEEP
Backa:
speed = -100
steer = 100

for i= 0 to 100
	pulsout  leftWheel,leftOff +speed+steer  
	pulsout rightWheel,rightOff-speed+steer  
	pause 10
next

speed = -100
steer = -100

for i= 0 to 100
	pulsout  leftWheel,leftOff +speed+steer  
	pulsout rightWheel,rightOff-speed+steer  
	pause 10
next

sleep 10
mcount = 0
goto init