Zumo 32U4 – Using the Proximity Sensors to Point Towards the Closest Object

Assuming you want to use the cute little Zumo for a Robot Sumo competition, you need to make the Zumo turn in the direction of the highest proximity.  This may be a competing robot, or in the case of Robofest BottleSumo, it may be a 2L bottle.  Either way, the faster you can identify and turn towards the closest object, the faster you can do something about it.  In this post, I discuss a simple way to turn to point to the closest object within the range of the proximity sensors.  There are lots of ways to do this, including the example that comes with the Zumo library.  The example is very elegant, but slightly abstract for my target audience of these blog posts.  For this post, I’ll take a very simple procedural approach.

The flow is:

• If the left proximity sensor is the strongest, rotate counter-clockwise until the front sensor with the left LED is the strongest.
• Else if the right proximity sensor is the strongest, rotate clockwise until the front sensor with the right LED is the strongest.
• At this point, we know that the left or right sensor are lower than the front sensor with either LED.  Therefore, if the front with the left LED is stronger, rotate counter clockwise until the front has the same returned signal for the left and right LED’s.  Similarly, if the front with the right is stronger, rotate clockwise until the front returns the same level with both the left and the right LED’s.

The easiest way to define the last step is to use a variable named “error” which is the difference of the front with right LED minus the front with left LED.  The function for this is shown below:

void turnToStrongestProx() {
  int left,frontLeft,frontRight,right;
  int error;

  proxSensors.read();
  displayProximity();
  left=proxSensors.countsLeftWithLeftLeds();
  frontLeft=proxSensors.countsFrontWithLeftLeds();
  frontRight=proxSensors.countsFrontWithRightLeds();
  right=proxSensors.countsRightWithRightLeds();
  if(left>frontLeft&&left>frontRight&&left>=right) {
    motors.setSpeeds(-TURN_SPEED,TURN_SPEED);
    do {
      proxSensors.read();
      displayProximity();
    } while (proxSensors.countsFrontWithLeftLeds()<left);
  }  else if(right>frontLeft&&right>frontRight&&right>left) {
    motors.setSpeeds(TURN_SPEED,-TURN_SPEED);
    do {
      proxSensors.read();
      displayProximity();
    } while (proxSensors.countsFrontWithRightLeds()<right);
  }
  do {
    proxSensors.read();
    displayProximity();
    error=proxSensors.countsFrontWithRightLeds()-proxSensors.countsFrontWithLeftLeds();
    if(error>0){
      motors.setSpeeds(TURN_SPEED,-TURN_SPEED);
    } else if(error<0) {
      motors.setSpeeds(-TURN_SPEED,TURN_SPEED);
    }
  } while (error!=0);
  buzzer.playFrequency(440, 200, 15);
  motors.setSpeeds(0,0);
} 

This isn’t the most elegant way of achieving this, but it gets the job done.  If there is no signal, the robot does nothing.  This function uses a constant that needs to be defined in the main program by

#define TURN_SPEED 200

You can adjust the value of TURN_SPEED to change the speed of rotation.  The function also calls a function named displayProximity().  This function simply displays the values of the proximity sensor on the LCD.

void displayProximity() {
  lcd.clear();
  lcd.gotoXY(0,1);
  lcd.print(proxSensors.countsLeftWithLeftLeds() );
  lcd.gotoXY(2,0);
  lcd.print(proxSensors.countsFrontWithLeftLeds() );
  lcd.gotoXY(5,0);
  lcd.print(proxSensors.countsFrontWithRightLeds() );
  lcd.gotoXY(7,1);
  lcd.print(proxSensors.countsRightWithRightLeds() );
} 

The rest of the program (minus the two functions described above) is:

#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4Buzzer buzzer;

#define TURN_SPEED 200

void setup() {
  proxSensors.initThreeSensors();
}

void loop() {
  lcd.clear();
  lcd.print("Press A");
  buttonA.waitForButton();
  lcd.clear();
  turnToStrongestProx();
}