<!DOCTYPE aesl-source>
<network>


<!--list of global events-->
<event size="0" name="start_calibration"/>
<event size="0" name="follow_line"/>
<event size="2" name="vars"/>


<!--list of constants-->
<constant value="2" name="CALIBRATION_STEPS"/>
<constant value="35" name="MAX_REFLECTION_VARIATION_PERCENTAGE"/>


<!--show keywords state-->
<keywords flag="true"/>


<!--node thymio-II-->
<node nodeId="7198" name="thymio-II">var calibrating = -1
var following_line = -1
#var min_ambiant[2] = [32767, 32767]
#var max_ambiant[2] = [-32768, -32768]
var min_reflected[2] = [32767, 32767]
var max_reflected[2] = [-32768, -32768]
var white_threshold[2] = [-32768, -32768]
#var black_threshold[2] = [32767, 32767]

# [((last) time, motor.left, motor.right), (pre-last), ...]
var calibration_values[CALIBRATION_STEPS * 3] = [0, 0, 0, 3000, 200, 200]
#[0, 0, 0, 3000, 100, 100, 2500, 100, -100, 3000, 100, 100, 2500, 100, -100, 3000, 100, 100, 2500, 100, -100, 3000, 100, 100]

var var1 = -1
var var2 = -1
var var3 = -1
var var4 = -1

var lastTurned = 0
var emergency = 0

timer.period[0] = 0
timer.period[1] = 0

sub start_calibration
	#min_ambiant = [32767, 32767]
	#max_ambiant = [-32768, -32768]
	min_reflected = [32767, 32767]
	max_reflected = [-32768, -32768]
	calibrating = CALIBRATION_STEPS
	timer.period[0] = 100
	return

sub update_calibration
	# min_ambiant
	#call math.min(min_ambiant[0], min_ambiant[0], prox.ground.ambiant[0])
	#call math.min(min_ambiant[1], min_ambiant[1], prox.ground.ambiant[1])

	# max_ambiant
	#call math.max(max_ambiant[0], max_ambiant[0], prox.ground.ambiant[0])
	#call math.max(max_ambiant[1], max_ambiant[1], prox.ground.ambiant[1])

	# min_reflected
	call math.min(min_reflected[0], min_reflected[0], prox.ground.reflected[0])
	call math.min(min_reflected[1], min_reflected[1], prox.ground.reflected[1])

	# max_reflected
	call math.max(max_reflected[0], max_reflected[0], prox.ground.reflected[0])
	call math.max(max_reflected[1], max_reflected[1], prox.ground.reflected[1])
	return

sub follow_line
	following_line = 1
	return

onevent prox
	# collision detection
	if (motor.left.target != -motor.right.target) and (((motor.left.target > 20 or motor.right.target > 20) and (prox.horizontal[0] > 3000 or prox.horizontal[1] > 3000 or prox.horizontal[2] > 3000 or prox.horizontal[3] > 3000 or prox.horizontal[4] > 3000)) or ((motor.left.target < -20 or motor.right.target < -20) and (prox.horizontal[5] > 3000 or prox.horizontal[6] > 3000))) then
		motor.left.target = 0
		motor.right.target = 0
		following_line = 0
		return
	end


	if calibrating > 0 then
		callsub update_calibration
	elseif following_line > 0 then
		var1 = prox.ground.reflected[0]
		var2 = prox.ground.reflected[1]

		lastTurned = 0

		if var1 &lt; white_threshold[0] then
			var1 = 500
			lastTurned = lastTurned - 1
		else
			var1 = 0
		end
		if  var2 &lt; white_threshold[1] then
			var2 = 500
		else
			var2 = 0
			lastTurned = lastTurned + 1
		end

		motor.left.target = var2
		motor.right.target = var1

		if var1 == 0 and var2 == 0 then
			if emergency == 0 then
				if lastTurned != 0 then
					emergency = lastTurned
					timer.period[1] = 1000
				end
			elseif emergency == 2 then
				# lost the line :(
				emergency = 0
				following_line = 0
			else
				# emergency turn
				motor.left.target = 200 * emergency
				motor.right.target = -200 * emergency
			end
		else
			emergency = 0
		end
	end
	return

onevent timer0
	if calibrating > 0 then
		if calibrating == 1 then
			var1 = (max_reflected[0] - min_reflected[0]) / 10 * MAX_REFLECTION_VARIATION_PERCENTAGE / 10
			white_threshold[0] = max_reflected[0] - var1
			var1 = (max_reflected[1] - min_reflected[1]) / 10 * MAX_REFLECTION_VARIATION_PERCENTAGE / 10
			white_threshold[1] = max_reflected[1] - var1
			timer.period[0] = 0
		end
		calibrating--
		timer.period[0] = calibration_values[calibrating*3]
		motor.left.target = calibration_values[(calibrating*3)+1]
		motor.right.target = calibration_values[(calibrating*3)+2]
	end
	return

onevent timer1
	if emergency != 0 then
		emergency = 2
		timer.period[1] = 0
	end
	return

onevent start_calibration
	callsub start_calibration
	return

onevent follow_line
	callsub follow_line
	return</node>


</network>