La logique de fonctionnement du robot

Voici le code chargé dans les 2 ko supérieurs de l'EEPROM du BSII-sx ; Il reste 14 ko de d'EEPROM ce qui permet des évolutions avenir.

le 26/02/2001                                                
' CoWbOyZ :  T / B                                               
'            o            o                         
'             \          /                           
'          |*****************|                                             
'  GAL  ---|o GAA  [1]  DAA o|---  DAL                         
'  (1)     |  (2)       (3)  |     (4)                      
'          |                 |                           
'  GML  ---|o GMA  [2]  DMA o|---  DML                          
'  (1)     |  (2)       (3)  |     (4)                     
'          |                 |                            
'  GRL  ---|o GRA  [3]  DRA o|---  DRL                         
'  (1)     |**(2)*******(3)**|     (4)       

dirs=$f000
x var byte
serpos var byte(12)
ser1 var byte
ser2 var byte
ANTD var bit
ANTG var bit
hist var nib(2)
nbav var byte
nbrand var byte

nbav = 0
nbrand = 0
RANDOM nbrand
nbrand = 20 + (nbrand/10)

vit CON 20
nb var byte

avancer CON 1
reculer CON 2
tourner_d CON 3
tourner_g CON 4
arreter CON 5

AVT CON 45
LEV CON 220
 
GAL CON 255
GAA CON 120
DAA CON 120
DAL CON 0
GML CON 255
GMA CON 118
DMA CON 120
DML CON 0
GRL CON 255
GRA CON 125
DRA CON 125
DRL CON 0

serpos(0) = GAL
serpos(1) = GAA
serpos(2) = DAA
serpos(3) = DAL
serpos(4) = GML
serpos(5) = GMA
serpos(6) = DMA
serpos(7) = DML
serpos(8) = GRL
serpos(9) = GRA
serpos(10) = DRA
serpos(11) = DRL
serout 15,17405,[%01110101]
serout 15,17405,[%01011010]
serout 14,17405,[%01110101]
serout 14,17405,[%01011010]
serout 13,17405,[%01110101]
serout 13,17405,[%01011010]
gosub servos
gosub tete
pause(1000)
'gosub tete_d
'pause(1000)
'gosub tete_g
'pause(1000)


demarre:
gosub antenne
pause(50)
if (ANTG=0) then demarre
pause(1000)
nb = 0
hist(0) = avancer

main0 :
nb = nb + 1
if (hist(0) = avancer) then avance
if (hist(0) = reculer) then recule
if (hist(0) = tourner_g) then tourne_g0
if (hist(0) = tourner_d) then tourne_d0
goto main0

avance:
nbav = nbav + 1
gosub tete
'avance 1
serpos(1) = GAA +  AVT
serpos(9) = GRA +  AVT
serpos(6) = DMA -  AVT
gosub servos
pause(vit)
'pose 1
serpos(0) = GAL +  LEV
serpos(8) = GRL +  LEV
serpos(7) = DML -  LEV
gosub servos
pause(vit)
'leve 2
serpos(3) = DAL +  LEV
serpos(11) = DRL +  LEV
serpos(4) = GML -  LEV
gosub servos
pause(vit)
'recule 1
serpos(1) = GAA -  AVT
serpos(9) = GRA -  AVT
serpos(6) = DMA +  AVT
gosub servos
'avance 2
serpos(2) = DAA -  AVT
serpos(10) = DRA -  AVT
serpos(5) = GMA +  AVT
gosub servos
pause(vit)
'pose 2
serpos(3) = DAL -  LEV
serpos(11) = DRL -  LEV
serpos(4) = GML +  LEV
gosub servos
pause(vit)
'leve 1
serpos(0) = GAL -  LEV
serpos(8) = GRL -  LEV
serpos(7) = DML +  LEV
gosub servos
pause(vit)
'recule 2
serpos(2) = DAA +  AVT
serpos(10) = DRA +  AVT
serpos(5) = GMA -  AVT
gosub servos
goto main0

recule:
gosub tete
serpos(1) = GAA -  AVT
serpos(9) = GRA -  AVT
serpos(6) = DMA +  AVT
gosub servos
pause(vit)
serpos(0) = GAL +  LEV
serpos(8) = GRL +  LEV
serpos(7) = DML -  LEV
gosub servos
pause(vit)
serpos(3) = DAL +  LEV
serpos(11) = DRL +  LEV
serpos(4) = GML -  LEV
gosub servos
pause(vit)
serpos(1) = GAA +  AVT
serpos(9) = GRA +  AVT
serpos(6) = DMA -  AVT
gosub servos
serpos(2) = DAA +  AVT
serpos(10) = DRA +  AVT
serpos(5) = GMA -  AVT
gosub servos
pause(vit)
serpos(3) = DAL -  LEV
serpos(11) = DRL -  LEV
serpos(4) = GML +  LEV
gosub servos
pause(vit)
serpos(0) = GAL -  LEV
serpos(8) = GRL -  LEV
serpos(7) = DML +  LEV
gosub servos
pause(vit)
serpos(2) = DAA -  AVT
serpos(10) = DRA -  AVT
serpos(5) = GMA +  AVT
gosub servos
goto main0

tourne_d0:
gosub tete_d
serpos(3) = DAL + LEV
serpos(11) = DRL + LEV
gosub servos
pause(vit)
tourne_d:
serpos(2) = DAA + AVT
serpos(10) = DRA + AVT
gosub servos
serpos(1) = GAA - AVT
serpos(5) = GMA - AVT
serpos(9) = GRA - AVT
serpos(6) = DMA - AVT
gosub servos
serpos(3) = DAL - LEV
serpos(11) = DRL - LEV
gosub servos
pause(vit)
serpos(0) = GAL - LEV
serpos(8) = GRL - LEV
serpos(7) = DML + LEV
gosub servos
pause(vit)
serpos(2) = DAA - AVT
serpos(10) = DRA - AVT
gosub servos
serpos(1) = GAA + AVT
serpos(9) = GRA + AVT
gosub servos
pause(vit)
serpos(0) = GAL + LEV
serpos(8) = GRL + LEV
gosub servos
pause(vit)
serpos(4) = GML - LEV
serpos(6) = DMA + AVT
gosub servos
serpos(5) = GMA + AVT
serpos(7) = DML - LEV
gosub servos
pause(vit)
serpos(4) = GML + LEV
serpos(3) = DAL + LEV
serpos(11) = DRL + LEV
gosub servos
pause(vit)
goto main0

tourne_g0:
gosub tete_g
serpos(0) = GAL - LEV
serpos(8) = GRL - LEV
gosub servos
pause(vit)
tourne_g:
serpos(1) = GAA - AVT
serpos(9) = GRA - AVT
gosub servos
serpos(2) = DAA + AVT
serpos(6) = DMA + AVT
serpos(10) = DRA + AVT
serpos(5) = GMA + AVT
gosub servos
serpos(0) = GAL + LEV
serpos(8) = GRL + LEV
gosub servos
pause(vit)
serpos(3) = DAL + LEV
serpos(11) = DRL + LEV
serpos(4) = GML - LEV
gosub servos
pause(vit)
serpos(1) = GAA + AVT
serpos(9) = GRA + AVT
gosub servos
serpos(2) = DAA - AVT
serpos(10) = DRA - AVT
gosub servos
pause(vit)
serpos(3) = DAL - LEV
serpos(11) = DRL - LEV
gosub servos
pause(vit)
serpos(7) = DML + LEV
serpos(5) = GMA - AVT
gosub servos
serpos(6) = DMA - AVT
serpos(4) = GML + LEV
gosub servos
pause(vit)
serpos(7) = DML - LEV
serpos(0) = GAL - LEV
serpos(8) = GRL - LEV
gosub servos
pause(vit)
goto main0

servos:
for x=0 to 3
	ser1 = %00000000
	ser2 = %10000000
	ser1 = ser1 | (x << 4)
	ser2 = ser2 | (x << 4)
	ser1 = ser1 |  serpos.lownib(2*x)
	ser2 = ser2 |  serpos.highnib(2*x)
	serout 15,17405,[ser1]
	serout 15,17405,[ser2]
	gosub choix_action
	ser1 = %00000000
	ser2 = %10000000
	ser1 = ser1 | (x << 4)
	ser2 = ser2 | (x << 4)
	ser1 = ser1 |  serpos.lownib(2*(x+4))
	ser2 = ser2 |  serpos.highnib(2*(x+4))
	serout 14,17405,[ser1]
	serout 14,17405,[ser2]
	gosub choix_action
	ser1 = %00000000
	ser2 = %10000000
	ser1 = ser1 | (x << 4)
	ser2 = ser2 | (x << 4)
	ser1 = ser1 |  serpos.lownib(2*(x+8))
	ser2 = ser2 |  serpos.highnib(2*(x+8))
	serout 13,17405,[ser1]
	serout 13,17405,[ser2]
	pause(1)
	gosub choix_action
next
return

antenne:
ANTG = IN6
ANTD = IN7
return

choix_action:
if (nbav>nbrand AND hist(0)=avancer) then megatourne
ANTG = IN6
ANTD = IN7

if (ANTG = 1 AND ANTD = 0 AND hist(0) = avancer) then jreculed
if (ANTG = 0 AND ANTD = 1 AND hist(0) = avancer) then jreculeg
if (ANTG = 1 AND ANTD = 1 AND hist(0) = avancer) then jreculeg

if (ANTG = 1 AND (hist(0) = tourner_g OR hist(0) = tourner_d)) then jreculed
if (ANTD = 1 AND (hist(0) = tourner_g OR hist(0) = tourner_d)) then jreculeg

if ((hist(0) = tourner_g OR hist(0) = tourner_d) AND nb > 20 ) then javance
if (hist(1) = tourner_g AND hist(0) = reculer  AND nb > 20 ) then jtourneg
if (hist(1) = tourner_d AND hist(0) = reculer  AND nb > 20 ) then jtourned
if (hist(0) = reculer AND nb > 20 ) then javancet
goto pipo

megatourne:
RANDOM nbrand
nbrand = 20 + (nbrand/10)
nb = 16
nbav = 0
hist(0) = tourner_d
goto tourne_d0

jtourned:
nb = 18
hist(0) = tourner_d
goto tourne_d0

jtourneg:
nb = 18
hist(0) = tourner_g
goto tourne_g0

jreculed:
nb = 18
hist(1) = tourner_d
hist(0) = reculer
goto recule

jreculeg:
nb = 18
hist(1) = tourner_g
hist(0) = reculer
goto recule

javance:
nb = 18
hist(0) = avancer
goto avance

javancet:
nb = 18
hist(0) = tourner_g
goto tourne_g0

pipo:
return

tete_g:
serout 15,17405,[%01001111]
serout 15,17405,[%11000011]
return
tete_d:
serout 15,17405,[%01000000]
serout 15,17405,[%11001100]
return
tete:
serout 15,17405,[%01000000]
serout 15,17405,[%11000111]
return