Embedded Motion Control 2013 Group 3

Contact info

Vissers, Yorrick (YO)	0619897	y.vissers@student.tue.nl
Wanders, Matthijs (MA)	0620608	m.wanders@student.tue.nl
Gruntjens, Koen (KO)	0760934	k.g.j.gruntjens@student.tue.nl
Bouazzaoui, Hicham (HI)	0831797	h.e.bouazzaoui@student.tue.nl
Zhu ,Yifan (YI)	0828010	y.zhu@student.tue.nl

Meeting hours

Mondays 11:00 --> 17:00
Wednesdays 8:45 --> 10:30
Thursday 9:00 --> 10.00 Testing on Pico

Meet with tutor: Mondays at 14:00

Planning

Ma 09 sept:

• Finish installation of everything
• Go through ROS (beginner) and C++ manual

Wo 11 sept:

• Finish ROS, C++ manuals
• Start thinking about function architecture

Ma 16 sept:

• Design architecture
• Functionality division
• Divide programming tasks

Do 19 sept:

• Finish "state stop" (Koen)
• Finish "drive_parallel" (Matthijs, Yorrick)
• Creating a new "maze/corridor" in Gazebo (Yifan)
• Simulate and build the total code using Gazebo (Hicham)
• Testing with robot at 13:00-14:00

Vr 20 sept:

• Finish "crash_avoidance"
• Coding "gap_detection" (Yifan)
• Coding "dead_end_detection" (Matthijs, Yorrick)
• Coding "maze_finished" (Koen, Hicham)

Ma 23 sept:

• Finish "drive_parallel"
• Putting things together
• Testing with robot at 12:00-13:00 (Failed due to network down)

Unfortunately we encountered some major problems with the Pico robot due to a failing network. We discussed the approach for the corridor competition. At this point the robot is able to drive parallel through the corridor and can look for gaps either left or right. We a gap is reached the robot will make a smooth circle through the gap. This is all tested and simulated. For the corridor competition we will not check for death ends. There isn't enough time to implement this function before Wednesday 25 September. The corresponding actions such as "turn around" won't be finished either. Without these functions we should be alble to pass the corridor competition successfully.

Di 24 sept:

• Testing with robot at 13:00-14:00
• Finding proper parameters for each condition and state

Wo 25 sept:

• Finish clean_rotation
• Finish gap_handling
• Putting things together
• Corridor Challenge

Week 5:

• Design and simulate code
  • Create a structure "laser_data" which processes the data of the laser. This contains the calculation of (Ma + Yo):
    • Shortest distance to the wall
    • Distance to the right wall with respect to Pico
    • Distance to the left wall with respect to Pico
  • Edit/improve state "drive_parallel" with a feedback controller according to the angle with respect to the right wall (Ma + Yo).
  • Create the condition "dead_end". When a dead end is detected switch to the state "turn_around" (Yi + Ko).
  • Edit/improve the condition "gap_detect_left/right". Make this condition more robust (Yi + Ko).
  • Start researching the properties of the camera of Pico (Hi).
• Testing on Pico (Listed by priorities)

1) Test robustness of the feedback controller and gap_detection
2) Tune the translational and rotational speed (faster)
3) Test detecting dead ends
4) Test strange intersections

Week 6:

• Design and simulate code
  • Edit/improve state "drive_parallel" with a feedback controller according to the angle with respect to the right wall (Ma + Yo).
  • Create the condition "finished". This is the condition when Pico exists the maze. Switch to state "finalize" (Yi + Ko).
  • Improve the priorities list. Pico needs to make smart decisions when multiply conditions occur. Basically the priorities are listed as (Ma + Yo):

1) Turn right
2) Go straight
3) Turn left

• • Continue researching the properties of the camera of Pico (Hi)
• Testing on Pico (Listed by priorities)

1) Test the state "turn_around"
2) Test the condition "finished"
3) Building a small maze containing a t-intersection and a dead end and test the priorities (Can also be done properly during simulation)

Week 7:

• Design and simulate code
  • Edit/improve the state "reset". This states handles the condition when a object is detected close to Pico.
  • Imaging processing. Detect arrows on the wall (Hi).
• Testing on Pico (Listed by priorities)

1) Build a maze and solve it without using camera.
2) Test camera

Week 8:

• Design and simulate code
  • Finalize the code
  • Imaging processing. Detect arrows on the wall.
• Testing on Pico (Listed by priorities)

1) Build a maze and solve it using camera.
2) Test camera

Progress

Week 5
(MA & YO) :

• Removed scanning functionality from the states and conditions and put it in a seperate function that returns a struct "Laser_data" containing info about wall locations and scanner range/resolution etc. Did some tuning on the gap_detection algorithm to make it more robust and made some minor changes on the decision tree. (now only checks nect condition if previous one returns false).
  
• Improved the decision tree to satisfy the required priorities for a wall follower strategy. The priorities for navigation are: 1) Take right gaps 2) Drive par 3) Take left turns. In order to only take left turns when there is nothing interesting ahead or to the right a new condition is introduced: go_straight_priority. When this condition returns false it is ok to take a left turn should there be a gap.
• Further improvements (w.r.t robustness) on gap_detection and dead_end_detection. Due to the new decision tree "number_of_spins" are now counted inside of the states (gap_handling), rather than inside the conditions.

(Testing on Pico: MA & YO & YI) : We learned that when there's a gap further ahead, Pico's laser doesn't see a clear jump in distance, but instead see several points (reflection?) within the gap. This is taken into account when designing for robustness.

• At the time of testing the priorities were not working as intended for the abovementioned reason, this has been improved later that day. Will be tested again next week.
• Speeds are tuned for now to x_vel = 0.2 in state_drive_par and z_rot = 0.15 in state_gap_handling and state_turn_around.
• Dead_end_detection worked as intended when there was a dead end, but sometimes also detected a dead end when there was none. This has been improved that same day. Also the distance when this condition checks in front has been improved. Will be tested again next week.
• state_turn_around was already finished ahead of scedule. Therefore it was already tested on Pico this week and is working as intended.
• feedback control in state_drive_parallel was not implemented yet, and therefore is also not tested and tuned. This will be done next testing session.
  

Week 6
(KO):

• Modified the condition "maze_finished". Pico will first check in front is there is anything closer then 2 meter (variable). When this is not the case it will continue checking a range of approximately 150 degrees ahead for obstacles. When there is still nothing closer then 2 meters, Pico is done!
• When the condition "maze_finished" is satisfied Pico will be in the state "finalize". It will move with a constant translation velocity of 0.3 [m/s] until anyone hit the emergency button.

Week 7
Week 8

Corridor Competition

We succesfully passed the corridor competition with the 2nd fastest time. A video impression is shared here: Corridor competition Theseus

PICO usefull info

• minimal angle = 2,35739 rad
• maximal angle = -2,35739 rad
• angle increment = 0,00436554 rad
• scan.range.size() = 1081

We have defined the following conventions:

• We use a right turning orthogonal base w.r.t Pico:
  • the pos x-axis is pointing forward w.r.t Pico
  • the pos y axis is pointing to the left w.r.t Pico
  • the pos z axis is then pointing up w.r.t.Pico
  • the pos z rotation is then CCW

Strategy

For navigating through the maze we use a "wall follower" strategy. This means that the robot will always stick to the right wall and always find it's way to the exit. At a later stage we will extend this base strategy with extra features such as the camera to detect instructions. The main robot controller can be in six states according to the conditions. The conditions are:

• Dead_end_detection
  • Input(s): (sensor_msg) LaserScan_data
  • Output(s): (Bool) dead_end_detect
  • The flag "dead_end_detect" is set when the robot measures a death end in front.
• Gap_detection (left and right are seperate but similar condition functions)
  • Input(s): (sensor_msg) LaserScan_data
  • Output(s): (Bool) gap_detect
  • The flag "gap_detect" is set to true when the robot is located at the center of the gap. Only gaps on the right-hand side will be noticed.
• Crash_avoidance
  • Input(s): (sensor_msg) LaserScan_data
  • Output(s): (Bool) chrash_detect
  • The flag "chrash_detect" is set when the robot measures an object closer then 30 centimeters over the entire laser range.
• Maze_finished
  • Input(s): (sensor_msg) LaserScan_data
  • Output(s): (Bool) finalize
  • This flag will be set to true when the robot exits the maze and detect no object closer then 2 meters around it.

Within these functions a flag is set when the robot is in a certain condition. All based on the data of the laser scanner. Based on the flag which is set the robot encounters a certain condition and will be in the corresponding state. The six states are:

• state_drive_parallel: Keep the robot at a fixed distance (setpoint) from the right wall while driving.
• state_gap_handling: When arriving at the center of a gap (to the right). Rotate in place untill parallel with new right wall.
• state_stop: Stops the robot when necessary.
• state_finalize: When no longer between two walls and no maze in front, abort.
• state_turn_around: when at a dead end, turn around.

The state functions will be called according to the flag and determines the linear and rotational velocities. We will use one node that "spins" and subscribes to the laser data, and publishes the velocities.

The figure below depicts the simplified architecture of the program at this point.

Software architecture

• We use one package named Theseus2
• We use a node theseus that spins on 20 Hz and subscribes to pico/laser and publishes velocities. When pico/laser publishes something theseus_controller is called that contains our main functionality.
• theseus_controller is the controller that runs several steps:
  

1) Process the laserdata for future use.
2) Gather conditions (each conditions is explained elsewhere on the wiki)
3) Based on the conditions a state will be chosen and executed. In some situations (f.e gap_handling) the previous state is considered to determine the current state (Markov chain principle)
4) The chosen state will set the velocities that can be published by the node.

• datastructures.h: all global variables and datastructures are gathered here.
• send_velocities: this functions is used for theseus to publish the velocities.
• states: each contains the functionality of a state.
• conditions: each checks a certain condition to determine the right state.
  

At a later stage a second node will be added that subscribes to the camera and processes this data and publishes data (bool arrowleft and bool arrowright) when an arrow is detected. At that point we want theseus to subscribe to this topic.

Conditions

Crash avoidance:
Pico is not allowed to hit the walls, so there when Pico gets too close to the wall, it should stop driving. This is carried out in the next loop:

• If the shortest distance from the wall to Pico is less than 25 cm, Pico return a stop command.

Finished:
When Pico exists the maze the condition "Finished" is reached. The following steps are carried out to check if Pico behaves in this condition:

• Read out the laser data from -90 to +90 degrees with respect to the robot
• Check if all distances are larger than 2 meters. When this is the case. Maze finished!

Gap detection left:
During the going straight state, if there exists a gap to the left side, the condition "Gap detection left" is reached. The following steps are carried out to check if Pico behaves in this condition:

• Pico will continue checking the laser date of left side between upper and lower index (variables) during going straight.
• If the distance within the upper and lower index is larger than the proudct of sensitivity (variable) and setpoint, a internal counter will start to count.
• As soon as the counter reaches 35, it's for sure that there is a gap to the left side. Pico will jump to state "gap handling" and start to turn.

Gap detection right:
This conditions is the same as gap detection left, but obviously for the right side.

Dead end detection:
When Pico facing a dead end, the condition "Dead end" is reached. The following steps are carried out to check if Pico behaves in this condition:

• Check if the condition "Dead end" is active. If active, keeps Pico turning around until Pico facing backward to the dead end.
• If the condition "Dead end" is not active, read out the laser data from -5 to +5 degree w.r.t the Pico heading direction.
• Check if the distance from -5 to +5 degree are shorter than sensitivity times setpoint to see if there is a dead end in the front. When this is the case, set Boolean "dead_end_infront" to TRUE.
• When "dead_end_infront" is TRUE, calculate the differences of two nearby laser distance from -90 to +90 degree with respect to the robot.
• When all the difference is less than 0.2 meters, Pico reaches dead end, jump to state "Turn around".

Arrow detection:
At a junction there is a possibility that an arrow is located at the wall. Pico can detect this arrow by using it's camera. When Pico detects the arrow it has to determine whether the arrows is point right or left and either turn right or left. ...to be continued...

States

Stop:
This state is being called when Pico is attend to crash. Currently it will stop and do nothing. According to the planning we will modify this state in week 7. It should contain a smart algorithm which can offer new opportunities to continue his way through the maze.

Exit:
This state ensures that Pico is driving straight out of the maze with a constant velocity.

Final:
State Final is called when Pico satisfies the condition Finalize. This state will ensure that Pico will stop moving by sending 0 to the wheels for translation and rotation.

Gap handling:
When gap handling is entered, a 90 degrees rotation to the left or right (based on the gap_detection conditions) is made. The overall shortest distance of the laser is used as a radius for the corner. The relation: x_vel = radius * z_rot is used to make a nice corner. When Pico might come close to a wall the radius will be smaller, so also the corner Pico makes will be smaller, which helps in the situation of a small gap in a wide corridor. At the same time a minimum radius is set to ensure that pico will not make the corner too small.

When gap handling is entered, Pico will remain in this state until the 90 degrees rotation is finished. This is done by pre-calculating the number of iterations this will take.

During week 6 we will improve this function to not use a hard 90 degrees rotation but instead take pico's current orientation into account. This will help to go around corners better, when Pico's orientation is rotated, in particular when several corners occur after each other.

Drive parallel:
Drive parallel is the state that allows Pico to drive between two walls. In this state a setpoint is calculated as the middle of the closest point to the left and right of pico. On top of that we define the deviation from this setpoint as the error, and the current rotation as theta, both relative to closest point left and right. By trying to minimize the error and theta we keep Pico in the middle of corridors which has the benefit that per default it tries not to hit any obstacles.

With these definitions seven situations are distinguished that require different velocities (translational an rotational) to converge to the setpoint quickly and in a stable manner. This is currently done by fixed values tuned on pico, but will be improved with feedback that is a function of the error and theta to improve performance. The trivial case is if both the error and theta are small. The other 6 situations are visualised here:

Turn around:
Pico is switched to this state when a dead end is detected. When Pico enters this state he will make a clean rotation on the spot of 180 degrees and is kept in this state untill finished rotating. Turning Pico is done by a simple open loop function. An angular velocity is send to the wheels and for a pre-calculated number of iterations corresponding to 180 degrees. We don't use any feedback to compensate slip and friction etc. In terms of accuracy this will not be the best solution but the small error due to slip and friction will be handled by the state drive_parallel which does contain a feedback controller.