WallBot – Version 2

For my next version of WallBot I mounted the sonic sensor onto a servo.

1200_1401_IMG_2958

The goal is to improve wall detection. With the fixed position sensor it was hard to deal with walls that were at an oblique angle. Being able to rotate the sensor should make it much easier to handle those situations; after all, it’s not rocket science.

The Build

Apart from mounting the servo/sensor, I also removed the proto-shield, lowered the second deck and replaced the 9V battery with a 2 cell lipo. The small lipo battery fits nicely under the microcontroller board, though it’s not shown in any of the images.

It was a bit of a conundrum as to how to mount the sensor on the servo but after a quick google I found an example where they used double-sided foam tape and so that’s what I did. It works surprisingly well:

1200_1401_IMG_2963

[Also, if you are an acute observer you might have noticed from the images that the motors have extra wires protruding. These are coming from quadrature encoders that have been added, though in this version of the bot they are not being used.] 

Apart from drilling holes and doing some filing to mount the servo, I also drilled new mounting holes for the Arduino board in order to move it back enough  for the sensor to have full range of motion.

1200_1401_IMG_2998

The HC-SR04 sonic sensor is connected to the 5V and GND pins for power and A0 and A1 for the trigger and echo signals.

The servo power is connected directly to the motor batteries.

1200_1401_IMG_2969

Probably not the best setup as the voltage will be higher than 6V on a fresh pack but I think the servo can manage. So far there has been no problems.

My one complaint about the Adafruit Motor shield is that while it does provide support for two servos it uses the Arduino to power them. Would have been much better if a regulated 5V had been used from its own power source instead. A shame they didn’t do that.

The servo control signal is routed via the white wire to pin 3 on the Arduino.

1200_1401_IMG_3002

In the next version of the bot I will be replacing the motor shield with a small dedicated motor controller on the lower deck. This will dramatically clean up the upper deck as all the motor wiring will stay below and only two signal wires will need to be connected to the Arduino.

The Code

Now for the code.

My initial plan was to modify the original WallBot V1 sketch and simply add code to sweep the servo back and forth. I used my own SS_Servorator library to control the servo movements and the SS_ServoTrim library to better adjust servo pulse timings.

I quickly learned a few things from the initial attempt:

  • Sweeping the servo too far left and right made wall detection less reliable and caused very erratic behavior in tight spaces. I ended up staying within a 45°-35° arc off center.
  • Sweeping too fast decreased the effectiveness of the sensor it seemed, though maybe there is noise interference from the servo too. I ended up slowing the forward speed so accommodate the slower sweep. I could do with slowing the sensor weep even more.
  • When WallBot detects a wall and starts to turn, it was important to stop the sensor sweep and instead get it to move slowly to a 30°-45° position, from center, towards the side the wall is on. This enables the turns to be very effective.

Another big issue had to do with the general way the code is written. It’s a simple super loop and there are forced delays to provide timing for the sensor reading logic and so on. The result is that the servo handling code is not getting called in a timely manner and this causes the servo movements to become jerky. Moving forward I’m going to have to use some king of real time multitasking system, like FreeRTOS or some derivative. An RTOS adds it’s own issues but in the end it greatly simplifies handling multiple discrete systems that are time critical.

Lots of room for improvement. I didn’t get to spend a whole lot of time tweaking. There is still the issue with oblique angles where the sounds will not bounce back to the sensor. I need to improve the turning logic further to handle those cases.

Anyway, here is the servo related sections of the final code, which is up on GitHub too:


#include <SS_Servorator.h>
#include <SS_ServoTrim.h>
#include
:
:
// Servo control using Servo, SS_Servorator and SS_ServoTrim libraries
#define NUM_SERVOS 1

Servo servo[NUM_SERVOS];
SS_Servorator sr(NUM_SERVOS);
SS_ServoTrim trim(NUM_SERVOS);
:
:
SS_Angle min_angle = SS_DEGREES(45);
SS_Angle max_angle = SS_DEGREES(155);
:
void setup()
{
 :
 :
 // assign PWM pins to servos
 servo[0].attach(3);

 // register servo handler
 sr.setServoHandler( update_servo, NULL);

 // set initial position and speed of servo
 sr.setServoTargetAngle( 0, SS_DEGREES(90));
 sr.setServoMaxVelocity( 0, SS_NORMAL_RATE*2 );

 // set up trims so servo ranges are correct
 trim.setServoPulseRange( 0, 500, 2500);

 // wait for servo to get into starting position
 while ( sr.getServoAngle(0) != SS_DEGREES(90))
 {
   sr.service();
 }

 //delay a second so we can verify the servo is correctly centered
 // if not use the trim to adjust center point.
 delay(1000);
 :
 :
}

void loop()
{
 :
 :
 control_servos()
 :
 :
}

:
:
// the servo handler for Servorator
void update_servo( SS_Index index, SS_Angle angle, void *data)
{
 // SS_Angle is in 1000th of a degree
 long time = trim.getServoPulseTime(index, angle);
 servo[index].writeMicroseconds( time );
}

:
:
void control_servos()
{
 if( state != TURNING )
 {

  SS_Angle target = sr.getServoTargetAngle(0);

  if( target != min_angle && target != max_angle )
  {
    // set target to the furthest angle
    if( target - min_angle > max_angle -target )
    {
      sr.setServoTargetAngle( 0, min_angle );
    }
    else
    {
       sr.setServoTargetAngle( 0, max_angle );
    }
  }
  else
  {

    SS_Angle angle = sr.getServoAngle(0);

    if ( angle <= min_angle)
    {
      sr.setServoTargetAngle(0, max_angle);
    }
    else if ( angle >= max_angle)
    {
      sr.setServoTargetAngle(0, min_angle);
    }
  }

  sr.setServoMaxVelocity( 0, SS_DEGREES(120));
 }

 // sr.service() needs to be called regularly so that
 // the servos are updated via update_servo()
 sr.service();
}

void turn()
{
 :
 sr.setServoTargetAngle( 0, dir == LEFT ? SS_DEGREES(155) : SS_DEGREES(45) );
 sr.setServoMaxVelocity( 0, SS_DEGREES(60));
 :
}

The Video

Made a short video of WallBat2 in action. Definite improvement on the previous version. I also like the new lower profile.

Advertisements

One comment

Comments welcome

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s