Thymio-Show 2014

Sources

Kanon

Der Kanon lässt drei Thymios einen Kanon singen. Dazu brauchen alle 3 Thymios eine SD-Card mit 3 Soundtracks: P0.wav, P1.wav, P2.wav Die Sound müssen 8bit und 8kHz sein. Jedes Soundfile enthält eine Strophe des Kanons. Der folgende code wird auf den ThymioII geladen:

<node nodeId="1" name="thymio-II">var fullspeed = 350
var fullspeedleft = fullspeed
var fullspeedright = fullspeed
var leftslowdown = 0
var rightslowdown = 0
var frontslowdown = 0
var leftspeed[64]
var rightspeed[64]
var unitvector[64]
var speedindex = 0
var sumleftspeed = 0
var sumrightspeed = 0
var stuckturntime = 0

var searchturn = 0
var lastsearchturn = 0
var searchturntime = 0
var light_intensity = 0

#sound variables
var sound_playing = 0
var currentSound = 0
var currentSoundTest = 0
var SingerNr = 0 #bits 1-2 
var EinsatzNr = 0 #bits 3-4
var NrSingers = 0 #bits 5-6
var SingerID      #bits 7-10
var RandomVar[1]

var SingerTest = 0
var i = 0

var go = 0

# variables for localComm
var incoming = 0
var incoming_SingerNr
var incoming_EinsatzNr
var incoming_NrSingers
var incoming_SingerID

var topled[8]

call math.fill(prox.comm.rx._payloads,0)
call math.rand(RandomVar[0])
if RandomVar[0] &lt; 0 then
	RandomVar[0] = -RandomVar[0]
end
SingerID = RandomVar[0] % 7 + 1

prox.comm.tx = 0
prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this
prox.comm.rx = 0 #receive this
call prox.comm.enable(1)

#initialize arrays
call math.fill(unitvector, 1)

#use timer0 to initiate u turn timer time in ms 
timer.period[0] = 15000
#use timer 1 to update bottom leds, disable here, start when robot is singing
timer.period[1] = 0


# anderer roboter hier
onevent prox.comm
	incoming = prox.comm.rx
	incoming_SingerNr = incoming &amp; 0x3
	incoming_EinsatzNr = (incoming >> 2) &amp; 0x3
	incoming_NrSingers = (incoming >> 4) &amp; 0x3
	incoming_SingerID = (incoming >> 6) &amp; 0xF

	# first Singer just plays sound
	if  SingerNr == 1 then
		if  sound_playing == 0 then
			call sound.play(currentSound)
			EinsatzNr = currentSound
			sound_playing = 1
		end
	end
	
	#sanity check
	if  SingerNr > NrSingers then
		NrSingers = SingerNr
		prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this
	elseif incoming > 64  then  # singerID immer > 0
	    #set new send value
	    prox.comm.tx = 0

		if incoming_NrSingers &lt; NrSingers then
			prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this
 		elseif  SingerNr == 0 then
 			# incoming NrSingers still 0, first encounter
			if incoming_NrSingers == 0  then
				# in the rare case that both robots have the same SingerID, create new and return
				if incoming_SingerID == SingerID then
					call math.rand(RandomVar[0])
					if RandomVar[0] &lt; 0 then
						RandomVar[0] = -RandomVar[0]
					end
					SingerID = RandomVar[0] % 7 + 1
					return
				elseif incoming_SingerID > SingerID then
					SingerNr = 1
					NrSingers = 1
				end
			elseif incoming_SingerNr == 1 then
				SingerNr = incoming_NrSingers + 1
				NrSingers = SingerNr
				prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this
			end
		elseif  SingerNr > 1 then
			#play on sync, EinsatzNr is the same for all robots 0-2
			EinsatzNr = incoming_EinsatzNr
			if currentSound != (EinsatzNr + SingerNr - 1) % 3 then
				currentSound = (EinsatzNr + SingerNr - 1) % 3
				call sound.play(currentSound)
			end			
	 	end	 

	 		 	
		#update Nrsingers if new singer is present
   		if incoming_SingerNr >= NrSingers then
   	 		NrSingers = incoming_SingerNr
   	 		prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this
   		end
   		#sync NrSingers
   		if incoming_NrSingers > NrSingers then
   	 		NrSingers = incoming_NrSingers
   	 		prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this 
	   end
	   if SingerNr &lt; 2 then # all singers oher than 1 just listen
		   prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6) #send this
	   end
	end
	#show value on topleds
	call math.fill(topled,0)
	for i in 0:7 do
		topled[i] = (prox.comm.tx >> i) &amp; 0x1
	end
	call leds.circle(topled[0], topled[1], topled[2], topled[3], topled[4], topled[5], topled[6], topled[7])


onevent buttons
	when button.center == 1 do
		go = 1 - go
	end

onevent timer0
	searchturn++

onevent timer1
	light_intensity = mic.intensity
	call leds.bottom.left(1+light_intensity/10, 2, 2)
	call leds.bottom.right(2, 1+light_intensity/10, 2)

onevent prox
	# Robot drives around searching for others until it has found one
	if SingerNr == 0 then
		if  searchturn != lastsearchturn then
			call leds.circle(16, 16, 16, 16, 16, 16, 16, 16)

			searchturntime++
			# right turn for uneven searchturn
			if searchturn % 2 == 1 then
				fullspeedright = fullspeed
				fullspeedleft = fullspeed / 3 
			else		
			# left turn for even searchturns
				fullspeedright = fullspeed / 3
				fullspeedleft = fullspeed
			end

			if searchturntime > 35 then
				searchturntime = 0
				lastsearchturn = searchturn
				fullspeedleft = fullspeed
				fullspeedright = fullspeed
				call leds.circle(0, 0, 0, 0, 0, 0, 0, 0)
			end
		end

		if  searchturntime >0 then
			call math.muldiv(leftslowdown, prox.horizontal[4]*2 + prox.horizontal[3]/2, fullspeedleft, 4500)
			call math.muldiv(rightslowdown, prox.horizontal[0]*2 + prox.horizontal[1]/2, fullspeedright, 4500)
		else
			call math.muldiv(leftslowdown, prox.horizontal[4] + prox.horizontal[3], fullspeedleft, 4500)
			call math.muldiv(rightslowdown, prox.horizontal[0] + prox.horizontal[1], fullspeedright, 4500)
		end
		call math.muldiv(frontslowdown, prox.horizontal[2], (fullspeedleft + fullspeedright)/2, 3000)
		call math.min(leftslowdown, leftslowdown, fullspeedleft)
		call math.min(rightslowdown, rightslowdown, fullspeedright)
		call math.min(frontslowdown, frontslowdown, (fullspeedleft + fullspeedright)/2)

		leftspeed[speedindex] = fullspeedleft - leftslowdown - frontslowdown
		rightspeed[speedindex] = fullspeedright - rightslowdown - frontslowdown

		call math.dot(sumleftspeed, leftspeed, unitvector, 0)

		# if the robot is stuck, drive backward turn for a while
		when sumleftspeed &lt; 50 do
			stuckturntime++
		end

		if stuckturntime > 0 then
			stuckturntime++
			leftspeed[speedindex] = -fullspeedleft
			if  stuckturntime > 15 then
				stuckturntime = 0
			end
		end

		if  go == 1 then
			motor.left.target = leftspeed[speedindex]
			motor.right.target = rightspeed[speedindex]
		else 
			motor.left.target = 0
			motor.right.target = 0
		end
		speedindex++
		speedindex = speedindex &amp; 0x3F
	elseif go == 1 then
	# Robot is singing (and dancing?)
		
		#use timer 1 to update bottom leds
		timer.period[1] = 100
		
		#stop driving
		motor.left.target = 0
		motor.right.target = 0

	else
		call sound.play(-1)	
		sound_playing = 0
		timer.period[1] = 0
	end


onevent sound.finished
	#start next Track only in Singer 1, others have to sync
	if  SingerNr == 1 then
		if currentSound &lt; 2 then
			currentSound++
		else
			currentSound = 0
		end
		if  go == 1 then
			call sound.play(currentSound)
			EinsatzNr = currentSound
			prox.comm.tx = SingerNr + (EinsatzNr &lt;&lt; 2) + (NrSingers &lt;&lt; 4) + (SingerID &lt;&lt; 6)
		end
	end