Adapted the code to make it easier for me to read. Found some bugs and corrected them (if( x =3 ) should be if(x == 3))
Changed some parameters on control_motion. Control_logic is too simple to work properly.
Switched to a very basic logic, with predictable results, to be able to debug the motion controller accurately and start building a logic strategy step by step.
Corrected a bug (not filling the right drive mode in control_logic)
Another drive_mode I wasn't setting properly
Wall follower now stops after losing wall it was following.
Bug correction
Corrected a bug in the forward behavior where the travelled distance wasn't being computed properly.
Tuned values. The controllers of the distance to the wall and the angle towards it should be stated explicitly.
Changed parameters.
Alignment with walls after finishing a behavior is now responsability of the 'None' behavior. Wall follower stops when stops seeing a wall (instead of moving a bit forward afterwards).
Added generic PID function to the motion controller. Integrated it with the rotation behavior.
Integrated the PID to control wall alignment in None behavior
Implemente PID in the wall follower (u_theta only).
Added the option to change PID parameters over ROS message
Wall follower follows the wall that the brain told him to follow.
Reset logic to a blank slate to implement new ideas.
Introducing brand new brain! Now with extra logic! Motion controller is a slave to the brain! Needs debug.
Corrected a bug in the wall follow where it didn't follow the wall.
Lots of changes: The motion controller will try to align the robot based on last know angle to walls when lost a wall from wall following (buggy). Implemented logic to do U-turns on open spaces (buggy - we need to add the possibility of moving different lengths). Overal logic works ok, but depends on successful execution of U-turns. Added the option to tune different PIDs in the motion controller via ROS messages (MOTION CONTROLLER NEEDS TO BE TUNED - URGENT!!!)