Us_servo_scan Updated with EasyGoPiGo3 and Distance Sensor

GoPiGo
,
1

I have created an updated version of the DI GoPiGo ultrasonic sensor servo scan code for the GoPiGo3 with DI Servo Package and DI Distance Sensor.

My code is available here, with a draft README.

This update also

  • adjusts printing scale factor selected by farthest object measurement,
  • explains dimension of axis ticks resulting from the scale factor
  • adds x and y dimension on map
  • adds a REVERSE_AXIS flag for users with non-Dexter Servos that rotate opposite
  • communicates “Closest Object” found in each scan
  • communicates basis for move or stay(quit program) decision
  • communicates distance moved between successive scans
  • logs scan for objects in output
  • adds a “Stopping to enjoy the view” pause to mark each collection/print cycle and allow perusal of the result map.

and adds optional debug printing of data along the processes of:
  1. collecting the sparse matrix of r-theta data points,
  2. conversion to cartesian coordinate data points,
  3. rotation and origin-translation of sparse data array for printing,
  4. creation of a non-sparse object/no-object binary value matrix
  5. selection of a print character for each point in the non-sparse matrix

Sample output from this version:

*** SCANNING ***
Map:                   75 cm
 ---------------------------------------------------
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|             o ooooooooooooooo                     |
|           oo            o    o                    |
|                              o                    |
|           o                  o                    |
|           o                  o                    |
|          o                                        |
|          o                  oo                    |
|          o                  o                     |
|          o                  oo                    |
|        o                +    o                    |
 --------------------------------------------------- 75 cm
Closest Object: 13 cm, Each '-' is 3 cm

*** STOPPING TO ENJOY THE VIEW ***

*** WE HAVE CLEARANCE TO MOVE 3.9cm ***

*** SCANNING ***
Map:                   50 cm
 ---------------------------------------------------
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|          o o oo   o  o     ooo o                  |
|      o o        oo oo ooooo    o                  |
|    o                                              |
|                                o                  |
|    o                           o                  |
|   o                            o                  |
|                                 o                 |
|   o                            o                  |
|                                                   |
|   o                            o                  |
|   o                            o                  |
|                                o                  |
|   o                     +      o                  |
 --------------------------------------------------- 50 cm
Closest Object: 12 cm, Each '-' is 2 cm

*** STOPPING TO ENJOY THE VIEW ***
*** WE HAVE CLEARANCE TO MOVE 3.6cm ***

*** SCANNING ***
Map:                   50 cm
 ---------------------------------------------------
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|            o                                      |
|           o  o  oo                                |
|           o       oooo                            |
|                       oooooooooooo                |
|            o            oo         oo  o          |
|            o                           o          |
|            o            +              o          |
 --------------------------------------------------- 50 cm
Closest Object: 8 cm, Each '-' is 2 cm

!!! FREEZE - THERE IS SOMETHING INSIDE MY PERSONAL SPACE !!!
2

Hi @cyclicalobsessive,

That’s uber cool! We never tried doing that.

Would you mind using part of your code to integrate into one of our projects? I also have the idea to update itself in real-time, without having to redraw the whole area again on the following lines, but rather make it like a stream.

How much time does a scan take?

Thank you!

3

The scan time is tunable with

samples=3			# Number of samples for each angle. Suggest 1-5 (more samples = more trust, more time)
num_of_readings=45		# Number of trusted readings to use

(I should have changed them into all-caps since they are used as configurable constants. )

Here is a scan heading into a 90deg corner with samples=1 and num_of_readings=18 that runs 4 seconds:

*** SCANNING ***
Map:                   25 cm
 ---------------------------------------------------
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                                                   |
|                         o                         |
|                                                   |
|                          o                        |
|                        o                          |
|                      o     o                      |
|                             o                     |
|                    oo        o                    |
|                   o           o                   |
|                  o                                |
|                 o             o o                 |
|                                                   |
|                 o       +        o                |
 --------------------------------------------------- 25 cm
Closest Object: 8 cm, Each '-' is 1 cm

!!! FREEZE - THERE IS SOMETHING INSIDE MY PERSONAL SPACE !!!

Since the TOF distance sensor seems to exhibit less variance than the ultrasonic sensor, and because the process of displaying the readings hides the actual reading precision, and the data accuracy is not integral to the functions the bot performs, the “samples” constant works fine at one sample at each reading angle.

The num_of_readings constant is used in conjunction with an assumed scan sector “magic-number 180”. The number_of_readings can be decreased slightly in the current version and the displayed map sometimes looks better than with the current 45 readings, but there seem to be some constraints on the value which I have not investigated.

I also have been thinking of ways to use the print_map() function in other programs. If I make the sector size configurable, the bot could collect three data points around the central axis and only print those three. This will be very useful in my “is_it_a_wall()” function that I am doing next. (criteria will be that the area of the triangle is less than sensor_accuracy times the range to the “triangle base” times half the “triangle base”).

The new ds_scan() function would be even more useful with the addition of SCAN_CENTER_ANGLE:

ds_scan(sector_size=180, number_of_readings=9, sector_center_angle=90, num_samples=1)

It would be interesting to modify the print_map() function to print_kmeans() which would display each cluster center point with a C and then have an optional parameter to switch on/of the printing of the cluster points. (This would be very useful for experimenting with tuning the cluster size.)

It goes on and on - I was thinking of a print_driveQueue() version of the print_kmeans() to show both the “objects” (cluster centers with two or four nearest cluster members) and the planned path in your IntelligentObjectAvoider.py

Making a streamable version implies the possibility of a “robot status web page” that would display the current servo position, current distance reading, last scan map with a “still valid / invalidated by motion / possibly invalid by time”, periodic camera image (annotated by recognitions), current motor speeds, a “driveQueueHistoryMap()” window, and the kitchen sink.

Can you get that done over the weekend? Sorry, did I mention I get obsessive? I’m about to switch gears for a week to learn to sew my wife a tunic. I need to limit my “robot dreams” a bit. I’ve not been sleeping all that great since I got my GoPiGo3. I’ve been dreaming about robot awareness algorithms and functions in my nightmares.

(BTW, I would be very pleased if the updated ds_wServoPkg_scan.py and README appeared under a GoPiGo3/Projects/ServoScan folder at some time in the future. I tried to update the README per DI conventions.)

1 Like
4

Hi @cyclicalobsessive,

Your passion for robotics is truly admirable and I’ve got to tell you that it can get obsessive for anyone trying this - it can get addictive at times :smiley:

Anyway, for the time being, I have something different on my plate, but once I’m done with that, I’ll look into this. Anyway, I just had an idea where I could get the program to scan the whole area (those 145-150 degrees that the servo can offer) in much less time - like in under a second. I’ll have to do some experiments before to be sure that’s feasible in the first place.

Thank you!

1 Like
5

I did some more updating and generalizing on this code: added selectable width printing from tiny to large, character aspect ratio scaling (so shapes and angles look correct), and more options in the comments.)

*** SCANNING ***

Map:                                  33 cm
 -----------------------------------------------------------------------------
|                                                                             |
|                                                                             |
|                                                                             |
|                                                                             |
|                                                                             |
|                                                  o     o       o            |
|              o       o   o    o   o     o    o                              |
|         o                            o                                      |
|                                                                             |
|                                                                             |
|          o                                                                  |
|                                                                             |
|                                                                             |
|          o                                                                  |
|                                                                             |
|         o                                                                   |
|                                                                             |
|                                      +                                      |
 --------------------------------------0-------------------------------------- 33 cm
Each '-' is 0.9 cm      Each '|' is 1.8 cm
Closest Object: 19 cm  Farthest Valid Object: 33 cm
Farthest Reading: 300 cm

*** PAUSING TO ENJOY THE VIEW ***