Latest Code revision

Changed  a lot:
- Color Detection
- Path Finding