During the week end, we welded the MB Leds with Alexis. It was quite long but eventually we have our own blocks.
Benjamin created the remote functionality; It works quite well. If a block turns of 180°, it will be the remote to change the program.
I fixed some problems in the algorithm with the MB Leds and I will continue until the presentation.
After firmware transmission Cédric tried the firmware upgrade already implemented on TP PCB by Benjamin but failed to implement it on MB Led blocks in due time and eventually stopped his work : In fact, we have few time left and we prefer to focus on the presentation. Now, he will work on an animations.
We now have our face tracking algorithm running on BeagleBoard. Since a tracking only based on face detection was to slow and not very intuitive (you always have to look the camera), we decided to use a blob tracking algorithm, which we initialize with the face detection algorithm.
First, Casper looks for a face. When it finds one, it learns the associated color histogram. After what it tracks the histogram (with the OpenCV Camshift function), for a few tens of frames. If it does not find a face again during the blob tracking, it stops at the end of the frames. Otherwise, it keeps tracking the « blob » face.
We adopt a multithread program : a thread looks for a face every second, and a thread is responsible for blob tracking when a face is found. The first thread is used to set a counter which is decremented by the second thread.
After having spent hours focusing on our PID into the Télécom ParisTech’s dancing room, we joined Télécom Robotic’s room in order to work on our implementation of Lucas and Kanade algorithm, because of a giant chessboard in green and red, perfect for image processing. We fixed Copterix in the air using some strings, and started test, with or without threshold, and it wasn’t really efficient…
We thought a moment about using ‘unfisheye’ opencv’s algorithm (see joined photo to understand why we wanted to use it), but it took 0.6 seconds per frame on our Gumstix…
What our camera actually sees: we can observe a 'fisheye' effect
Then we stopped and decided we should decide exactly how, even if it worked, Lucas and Kanade would help us to correct Copterix’s drift.
As we have 10 frames per second when processing the algorithm, it will be really difficult to determine in real time if we actually corrected the drift whenever we would want to correct it, and that is why we imagined the following algorithm:
wait for Copterix to be horizontal (pitch and yaw < epsilon)
take a reference picture and search the good features to track on it (quite heavy operation)
while Copterix is horizontal (pitch and yaw < epsilon)
____take a picture, compare it with reference
____if not horizontal (pitch and yaw >= epsilon)
______wait for Copterix to be horizontal (pitch and yaw < epsilon)
______go to 3.
____if drift is big enough (move size > threshold)
______go to 12.
____if we don’t find enough features to track
______go to 1.
ask PID to incline Copterix toward the good direction (we have to decide an angle)
wait a chosen time (we have to be over-precise on this data)
ask PID to set Copterix horizontal
go to 1.
The real difficulty of this algorithm is that it infers we do own a powerful and precise PID, able to remain horizontal most of the time, and to follow an order in the better and faster possible way, which is of course not true.
That is why we are now considering the fact that we may not have time to have a excellent PID AND implement such a precise algorithm; we will talk about it tomorrow with our teachers.
Today, we continued on our recent work : I worked on the algorithm and on the rotation of a block. The rotation works quite well now. The algorithm for this rotation is simple : When a communication between two blocks is stopped (due to the left of someone), the two blocks save their « interfaces » (aliases of the UARTs ). When there is a PING from a block, we check the saved interfaces in order to see if the block was a « friend » of this block. If it’s the case, we just calculate the rotation of the block.
The main problem is that a block can communicate with 4 others blocks at the same time. So, sometimes, we have to wait a little bit when we want to compare the interfaces.
Cédric noticed in the afternoon a bug in the IrDA functions and we fixed this. Now, the IrDA has more reliable and there are few problems in the network.
I still have problems when two networks meet with more than one block each involved in the « deal » but things are going better each day.
Benjamin worked on the launcher task. He thought of a remote block who can control the animation/game that will be played soon after.
Cédric continued the firmware transmission and the protocol is actually functional at good speed. But he is now facing problems with flash writing.
Here is a little video of the synchronization (running on the GLiPs but the MBLed are arriving soon):
Every block knows its i,j and minI, minJ, maxI, maxJ in the network. The block in the north-west shows a M; north-east a B; south-west LE: south-east D. If he is alone, he shows a M too.
That’s why sometimes in the video, there are 2 M : one of the block has encountered problems and reset himself.
For the synchronization, the leader gives the time every 20 PING (approximately every second), when a block receive that, it transmits it to his neighbor and so on.
Since yesterday, Guillaume worked on algorithm reliability, improving its performance and solving some problems such as information sharing (number of blocks in network, position…). Some loop-back which were over-charging the network have been deleted. He worked with Benjamin to improve the algorithm but some problems remain unsolved… In late afternoon, the algorithm was working rather well : in a 9 blocks network, blocks detected the good number of blocks, their position, the leader ID, the positions of the others in many cases. But sometimes, one block loses his IrDA connection so he has to start again the algorithm. When dividing a network in 2 networks, the blocks graph (developped with Benjamin) gives the blocks good informations.
Benjamin worked on improving the snake game, now it has a tail and if you cut it (meaning you disconnect blocks before the tail leaves) you lose. Some apples have shown their faces on the screen, but we must handle turn detection before evolving furthermore. He also worked with Guillaume on the implementation of a blocks graph in order to react faster when a block leaves the network. In fact, if a member leaves, blocks are able to calculate that every members of the branch also left. At last he tested synchronisation implemented by Guillaume displaying an animation on MB Leds, it is pretty quick and all blocks flash at once.
I have first worked on improving some aspect of IrDA like variable waiting time before a resend and size of packets in order to improve the payload . I have continued to implement firmware transmission and have begun testing it. Transmission seems a bit slow and tomorrow I will look for a way to improve its speed.
Since our last post, Guillaume is still on the algorithm. Benjamin developed a display task for the old and new GLiPs. Now, we can see directly what’s going on in the network and try to debug it (see the picture). Yesterday, Guillaume tried to resolve bugs of freezing GLiP while the algorithm is running. He had to change many things in order to fix this and the only remaining freeze happens when some blocs leave the network.
There are some troubles when a block isn’t gone but don’t communicate with his neighbors. The biggest problem is when a block is rickety and that he communicates for time to time : he is considered as gone and soon after, he is here, etc. When this happens, the network can fail.
A little video to illustrate the visual debug:
Benjamin is develloping a special snake game concept for our system.
Today, I finalized the emission of the commands for the motors, from the Gumstix to the STM32 and their reception (check the validity of the packet and if it’s valid, update the commands sent to the motors).
Then, with Samuel, we added 0MQ exchanges on the Gumstix between Kalman and the PID and between the PID and the UART.
On the Gumstix, the program that handles the UART has two threads. One receives data from the sensors from the STM32 and sends it -thanks to 0MQ- to the program computing Kalman. The other one, receives the commands of the motors from the PID and sends them to the STM32.
We also attached the PCBs on the copter:
We configured Kalman with the new axes and the offset of roll and pitch angles.
Finally, we all tested the PID. The algorithm seems okay, however, we’ll have to set each coefficient properly.
We’re trying to control roll and pitch angles with a low thrust (in order to prevent any accident). Then, we’ll add a control on Z thanks to the Sharp sensor.
It is time to make a debriefing about last week achievement.
Colour gradient and intensity
Benjamin worked on the Led driver in order to display a picture and has fixed some problems with luminosity. He balanced green, blue and red and we are now able to display a white colour on our blocks (although a blue tint remains on videos). From now on, we also have a linear gradient of intensity for each primary colour. In order to obtain this gradient, we first tried to put a linear variation of intensity in registers, but it appeared linearity didn’t worked and he had to adjusted individually each step.
Electronic visual display:
An interruption is raised with a frequency of 10kHz and each time a new line is displayed. As our Led matrix has 8 lines, we have a electronic visual display at 1,25kHz. Each time we want to display a new line we connect to the Led driver through SPI sending it thanks to a vector of 288bits matching a 8 pixel line with three colour and 12 bit of gray control. Actually we only use 4 bit for each colour.
Yesterday I implemented acknowledgement system with IrDA transmission. First I needed to divide our sending queue into 2 queues: one for quick packets which don’t need ack such as PING, PONG and synchronizing messages sent at high frequency and allowing some losses; the other one is for important packet needing a safe transmission. These packet are sent to a task which could be seen as a buffer sending a packet (every 40ms) until it receives an ACK and then picking up the next one. In order to identify a packet an ACK transmit the ID of the sender and the ID of the packet it acknowledge and when an ACK packet is received, a message containing these information is send to the task managing output packets. In order to avoid loop waiting for an ACK , we use a counter and if a packet is not acknowledged after 10 tries we consider it as a lost packet (for ping messages, if we don’t receive one PONG for 10 of our PING, we consider the neighbour as gone). Once a neighbour is gone we empty the sending queues for this UART.
This system is not optimal, being slow, but at higher speed we got a lot corruption during transmission and more packet are lost.
We decided to rethink our algorithm after the reactions of yesterday’s post. We changed the algorithm when some block is leaving the network. If someone is missing, the block which notice that says it to the network. Every other block answer by « I’m still here ». If the leader doesn’t answer (probably he’s gone), there is an other election; those who don’t answer are considered to be out of the network. In this algorithm, the blocks keep in memory their position and orientation and we don’t have to start an election every time some block quits.
A little video showing up some features as the use of LED driver, election and direction:
NOTE: Now once a direction is decided, it is kept when the connection is lost.
As Samuel noticed that there is not formal description of the algorithm, I will do this now.
First of all, all the messages use the packet structure giving by Cédric here. So, they all have an ID and we know who is the sender. I will start by showing all the messages and then I’ll explain the algorithm.
List of the differents messages
There are a large number of messages and they all have specifics arguments.
For the algorithm, I use « interfaces » which are aliases of UARTs. The UARTs are used by the IrDA while the interfaces are used for the algorithm. For example, interface 0 represents the north (and is in reality UART 2), interface 1 is the east (but in reality it’s UART 5), etc.
All the blocks know :
their own ID (given by the STM32_ID),
their leader ID,
their orientation (which « interface » represents the north)
the number of member in their network
their « nearest » neighbors (who is in front of my UART2,3,4 and 5) : the Interface table.
They have a network table (for each ID, they know the position (i,j) and who was the leader when they had the information) and an election table (for each ID, they know his state in the election) . We will try to replace theses tables by a list in the future.
Every message except PING and PONG is answered by an ACK message. The PONG message contains the PING ID so it is itself a ACK. If there is no ACK, the message is resend.
When starting all of this, the block starts the timer 5 of the STM32 for the synchronization. (see section 4)
Each block sends PING messages in the 4 directions and start a timer for each direction.
When receiving a PING message, a block sends back a PONG message as fast as he can.
When it receives a PONG message, the block knows who is his neighbor and changes his Interface table. Furthermore, the block stops the timer.
If the block is aware that there was a neighbor but there is no answer at the end of the timer, it sends a RESET_NETWORK message (see section 5).
If this interface wasn’t ready before, the block has to check if he knew the neighbor (in order to notice a turn of the other block) or not (in order to start an election.)
Explanation of this check :
When receiving a PONG message from a « new neighbor », the block checks if the « new neighbor » was a neighbor before or not. This is checked by comparing the current interface table and the saved interface table.
If the interfaces tables don’t match, there is an election.
Else, the block sends to the block TURN (myID, rotation). The rotation is the difference of index between the current interfaces and the saved interfaces.
To compare the interfaces, the block waits a little bit for PONG message from all the interfaces.
3. The election
When it’s the first PONG from a new neighbor, an election takes place.
When starting an election, the block starts a timer. If his candidature isn’t refused at the end of the timer, he is elected and he broadcasts the information.
In the election, the blocks send messages for their « network » (all the blocks with the same leader). Here, the « network » can be a network of one block (so the leader ID is his ID).
The block sends a START message to his neighbors not in his « network » (and this is broadcast). All blocks sends a CANDIDATE message with his leaderID and the numbers of members in his network.
The winner of the election is the network with the larger number of blocks in his network. If this isn’t enough to decide , the lower leader ID is declared winner.
When receiving a CANDIDATE message, the block checks if the candidate can win or not. If he can’t, the block broadcasts a REJECTED message. If he cans, the block broadcasts the CANDIDATE message.
When receiving a REJECTED message with his leader ID, the block stops the timer : he can no longer win the election.
At the end of the timer, one network is the winner. It sends the ELECTED message and everyone broadcasts it.
4. After the election
After the election,the leader is in (0,0). If he was part of a network, he sends his information to the others by sending NETWORK messages.
For every NETWORK message received, the block analyzes it : if the leaderID of the NETWORK message isn’t his leaderID, it drops the message. Else, if the message contains a good information, the block changes his network table and transmits it.
Then he sends the POSITION message : [YOUR_POSITION leaderID, myOrientation, yourI, yourJ, myInterface]. The block which receive this calculate his new orientation (with « myOrientation », « myInterface ») and change his position in his network table. Then it calculate the position of his neighbors and sends the message.
The leader sends TIME messages (which are broadcasted if they are « young » enough.. That explains the synchroID) from times to times. In his TIME message, there is his current time (according to the STM32). The block that receive this message update his new « current time » (with a function which take into account the emission time) and then sends his current time to others, etc.
5. If someone leaves
If some block leaves the network, there is no PONG message to the PING message of his former neighbor. The former neighbor notices that and broadcasts a RESET_NETWORK message.
This message is broadcast to all the network and means that every data (leaderID, network table, …) must be restarted; the interface table is saved (for the TURN mechanism). A new election starts soon after. (plus the orientation, network information, synchro…).
The turn mechanism isn’t implemented yet and the ACK mechanism has to be implemented very soon.
Then, there will be messages for the applications (games, firmware upgrade, …).