# By Georges Saliba  2016-09-20,
# sous licence creative Commons CC-BY-NC-SA. 
# Vous pouvez copier et coller ce code dans aseba studio pour thymio. 
# permet d'avancer, tourner, et donnant au maximum 40 instructions au robot. 
# la notice d'utilisation est en pdf sur le site. 
# vous pouvez modifier les valeurs de A_TIME, T_TIME et T_TIME_G en fonction de votre robot

# CONSTANTES 
# Nombre maximum de déplacements  
var MAX_DEP = 40
# codage avancer 
var A = -1
# Durée en ms du déplacement vers l'avant  à la puissance 100  
var A_TIME = 4500
# code rotation sur soit-même à droite 
var D = -3
# code rotation sur soit-même à gauche 
var G = -2
# Durée d'une rotation sur soit même à droite en ms 
var T_TIME = 2300
# Durée d'une rotation sur soit même à gauche en ms 
var T_TIMEG = 2300
# Choisir des valeurs entre -5 et 5 pour rectifier la puissace des moteurs droits et gauche si le robot ne va pas tout droit 
# Choisir des valeurs entre -5 et 5 pour rectifier la puissace des moteurs droits et gauche si le robot ne va pas tout droit  
var delta_D = 0  
var delta_G = 0

# variables for state
var state[4] = [0,0,0,0]
var new_state[4] = [0,0,0,0]
var temps=0
var temps_timer=0
var etat = 2
var puis = 0 
# POur des raisons de compatibilité du fichier texte avec aseba, nous avons remplacé var commande[MAX_DEP] par var commande[40]
var commande[40]
var i = 0 
var iter = 0 

# reset outputs
call sound.system(-1)
call leds.top(0,0,0)
#robot temoin est vert en dessous
call leds.bottom.left(0,32,0)
call leds.bottom.right(0,32,0)
call math.fill(commande,0)

if  etat == 2 then
	etat = 0
	i = 0 
	call math.fill(commande,0)
end

sub mouvement 
	
	if  i < iter  then
		call leds.top(32,32,32)
	    call leds.circle(32,32,32,32,32,32,32,32)
		if  commande[i] == D then
			motor.left.target = 100 + delta_G
			motor.right.target = -(100 +delta_D)
			timer.period[0] = T_TIME 
		end
		if  commande[i] == G then
			motor.left.target = -(100+delta_G)
			motor.right.target = 100+delta_D
			timer.period[0] = T_TIMEG 
		end
		if  commande[i] == A then
			motor.left.target = 100 + delta_G
			motor.right.target = 100 +delta_D
			timer.period[0] = A_TIME 
		end
	end

sub change 
	etat = 1
	call leds.circle(0,0,0,32,32,32,0,0)

onevent buttons
	when button.right == 1 and etat == 0 do
		call leds.top(32,0,0)
		call leds.circle(0,32,32,32,0,0,0,0)
		if iter < MAX_DEP then 
			commande[iter] = D
		end	
	end
	when button.left == 1 and etat == 0 do
		call leds.top(32,0,0)
		call leds.circle(0,0,0,0,0,32,32,32)
		if iter < MAX_DEP then 
			commande[iter] = G
		end
	end
	when button.backward == 1 and etat == 0 do
	    etat = 1 
	    callsub mouvement
	    call leds.top(0,32,0)
	    call leds.circle(32,32,32,32,32,32,32,32)
	end

	when button.forward == 1 and etat == 0 do
   	    call leds.top(0,0,32)	
		if iter < MAX_DEP then 
			commande[iter] = A
		end
		call leds.circle(32,32,0,0,0,0,0,32)		
	end

	when button.center == 1 do
		call math.fill(commande,0)
		motor.left.target = 0
		motor.right.target = 0
		iter = 0 
		i = 0 
		etat = 0  
		call leds.top(0,0,0)
		call leds.circle(32,0,0,0,0,0,0,0)
	end


		
onevent prox 
	when prox.horizontal[5]>=3000 and prox.horizontal[6]>= 3000  and etat == 0 do 
		if iter < MAX_DEP then
			if commande[iter] != 0 then
				iter = iter +1 
			end
			call leds.circle(0,0,0,32,32,32,0,0)
		end
	end

	
onevent timer0
		timer.period[0] = 0
		motor.left.target = 0
		motor.right.target = 0
		timer.period[1] = 50		

onevent timer1
		call leds.top(0,0,0)
		timer.period[1]=0	
		i = i + 1
		callsub mouvement