Let's Make Robots!

Herpe

Try to get a certain distance from an object

Herpe is a curious-yet-skittish-puppy-bot. What he's supposed to do is seek you out and drive up to you, but if he gets too close, he backs away in fear. In reality, his motor is too fast and his CPU (a Parallax Basic Stamp 2) is too slow, so he doesn't react fast enough to realize he's getting close to you until he's bumped into you.

He also has trouble going in a straight line. After failing to solve the problem with code tweaks, I tried adding a 2-axis accelerometer to the board to let him detect when he's drifting off course and correct, but (unsurprisingly) the tiny bit of lateral acceleration felt when it turns slightly isn't enough to stand out of the background noise.

It was a fun experiment, and it was a great way to practice interfacing with a PING))) sensor and an accelerometer and servos, but really all it has taught me is that an R/C car platform is not the most convenient base for a robot. I've reused the Basic Stamp board and PING))) sensor in my next project, a differential-drive-based robot that I'll post here soon.

 

Rear view

 


The basis of the robot is an old electric R/C buggy that I've had laying around since I was a kid. The throttle and steering servos are interfaced directly into the Basic Stamp, and the R/C receiver is bypassed. Distance measurement is done by the Parallax PING))) sonar sensor, mounted directly onto the Basic Stamp's breadboard (so it has a fixed straight-ahead view). There's also a 2-axis accelerometer on the breadboard. The big yellow block is actually a Lego Mindstorms brick, but it's not connected to anything -- it's only there to hold the Basic Stamp board up high enough that the sonar sensor can see over the shock towers.

 


Code:

 

' {$STAMP BS2}
' {$PBASIC 2.5}

'----( setup )---------------------------------------------
' pin assignments
rangefinder         PIN     0
accel_y             PIN     4
accel_x             PIN     5
accel_temp          PIN     6
THROTTLE_SERVO      CON    14
STEERING_SERVO      CON    15


STEERING_SERVO_MIN_PULSE     CON   610
STEERING_SERVO_MAX_PULSE     CON   900

THROTTLE_SERVO_MIN_PULSE     CON   500
THROTTLE_SERVO_MAX_PULSE     CON   900

' for PULSOUT
IS_HIGH             CON     1
IS_LOW              CON     0


' rangefinder settings
RF_TRIGGER          CON     5   ' 10 uS
RF_SCALE            CON  $200   ' raw x 2.00 uS
RF_RAW_TO_IN        CON   889   ' 1 / 73.746 (with **)
RF_RAW_TO_CM        CON  2257   ' 1 / 29.034 (with **)

' accelerometer settings
ACCEL_SCALE         CON  $200   ' raw x 2.00 uS
TURNING_THRESHOLD   CON    50


' throttle settings
REVERSE_1           CON   100
STOPPED             CON    70
FORWARD_1           CON    58
FORWARD_2           CON    40
FORWARD_3           CON     0

' steering settings
LEFT                CON     0
SLIGHTLY_LEFT       CON    50
CENTERED            CON    60
SLIGHTLY_RIGHT      CON    70
RIGHT               CON   100

SLIGHT_STEERING_ADJUSTMENT      CON   10


'----( move_servo subroutine vars )-------------------------
position                        VAR   Word
servo_to_move                   VAR   Word
servo_min_pulse                 VAR   Word
servo_max_pulse                 VAR   Word

servo_pulse                     VAR   Word
move_servo_loop_counter         VAR   Byte

current_steering_position       VAR   Byte
current_throttle_position       VAR   Byte


'----( measure_distance subroutine vars )--------------------
distance                        VAR   Word


'----( read_x_force vars )------------------------------------
x_force                         VAR   Word
x_force_calibration             VAR   Word


'----( read_y_force vars )------------------------------------
y_force                         VAR   Word
y_force_calibration             VAR   Word


'----( general vars )----------------------------------------
i                               VAR   Byte



'====( initialize )===========================================
initialize:
  ' reset all servos
  GOSUB stop_moving
  GOSUB steer_center

  ' obviously, calibrate accelerometer
  GOSUB calibrate_accelerometer



'====( MAIN )=================================================
main:
  ' start main loop
  DO

    ' steer to correct any drift
    GOSUB read_x_force

    ' since the values are unsigned, going below zero actualy cycles back up to the

    'top of the 0-65535 range. A quick way to check if it's 'negative' is to see if the highest-order bit is a 1.

   IF     (x_force.BIT15 = 0) AND (x_force > TURNING_THRESHOLD) THEN
       GOSUB steer_slightly_left
    ELSEIF (x_force.BIT15 = 1) AND (x_force < (-1 - TURNING_THRESHOLD)) THEN
       GOSUB steer_slightly_right
    ENDIF


    ' measure how far away an obstacle is
    DO
      GOSUB measure_distance
    LOOP WHILE distance = 0

    'DEBUG ? distance, CR

    ' decide what to do based on the distance
    IF distance > 48 THEN
      GOSUB go_forward_medium
    ELSEIF distance > 36 THEN
      GOSUB go_forward_slowly
    ELSEIF distance > 18 THEN
      GOSUB stop_moving
    ELSE
      GOSUB go_backward_slowly
    ENDIF
  LOOP

  END



'====( steer_slightly_left )===============================
steer_slightly_left:
  DEBUG "Steering slightly left..."
  servo_to_move = STEERING_SERVO
  position = current_steering_position - SLIGHT_STEERING_ADJUSTMENT
  GOSUB move_servo
  RETURN


'====( steer_center )======================================
steer_center:
  DEBUG "Steering center...", CR
  servo_to_move = STEERING_SERVO
  position = CENTERED
  GOSUB move_servo
  RETURN


'====( steer_slightly_right )===============================
steer_slightly_right:
  DEBUG "Steering slightly right..."
  servo_to_move = STEERING_SERVO

  position = current_steering_position + SLIGHT_STEERING_ADJUSTMENT
  GOSUB move_servo
  RETURN


'====( go_backward_slowly )================================
go_backward_slowly:
  DEBUG "Backing up.", CR
  servo_to_move = THROTTLE_SERVO

  position = REVERSE_1
  GOSUB move_servo
  PAUSE 2

  position = STOPPED
  GOSUB move_servo
  PAUSE 50

  RETURN


'====( stop_moving )=================================
stop_moving:
  DEBUG "Stop.", CR
  servo_to_move = THROTTLE_SERVO

  position = STOPPED
  GOSUB move_servo

  RETURN


'====( go_forward_slowly )=================================
go_forward_slowly:
  DEBUG "Forward slow.", CR
  servo_to_move = THROTTLE_SERVO

  position = FORWARD_1
  GOSUB move_servo
  PAUSE 5

  position = STOPPED
  GOSUB move_servo
  PAUSE 50

  RETURN


'====( go_forward_medium )=================================
go_forward_medium:
  DEBUG "Forward medium.", CR
  servo_to_move = THROTTLE_SERVO

  position = FORWARD_1
  GOSUB move_servo
  PAUSE 10

  position = STOPPED
  GOSUB move_servo
  PAUSE 50

  RETURN


'====( move_servo )========================================
move_servo:
' Have to do some crazy math since we're dealing with unsigned integers. A value of -1 

' actually turns into 65534

IF     (position > 32767) THEN
    DEBUG "Position too far (", DEC position, "), setting to 0) "
    position = 0
  ELSEIF (position > 100) THEN
    DEBUG "Position too far (", DEC position, "), setting to 100) "
    position = 100
  ENDIF

  ' calculate servo pulse from 0-100 position
  IF servo_to_move = STEERING_SERVO THEN
    servo_min_pulse = STEERING_SERVO_MIN_PULSE
    servo_max_pulse = STEERING_SERVO_MAX_PULSE
    current_steering_position = position
  ELSEIF servo_to_move = THROTTLE_SERVO THEN
    servo_min_pulse = THROTTLE_SERVO_MIN_PULSE
    servo_max_pulse = THROTTLE_SERVO_MAX_PULSE
    current_throttle_position = position
  ELSE
    DEBUG "ERROR: Invalid servo requested (", servo_to_move, ")", CR
    RETURN
  ENDIF

  servo_pulse = servo_min_pulse + (position * (servo_max_pulse - servo_min_pulse) / 100)

  IF servo_pulse < servo_min_pulse THEN
    servo_pulse = servo_min_pulse
  ELSEIF servo_pulse > servo_max_pulse THEN
    servo_pulse = servo_max_pulse
  ENDIF

  FOR move_servo_loop_counter = 1 TO 30
    PULSOUT servo_to_move, servo_pulse
    PAUSE 7
  NEXT

  RETURN


'====( measure_distance )====================================
measure_distance:
  ' This subroutine triggers the PING))) sonar sensor and measures
  ' the echo pulse. The raw value from the sensor is converted to
  ' microseconds based on the Stamp module in use. This value is
  ' divided by two to remove the return trip -- the result value is
  ' the distance from the sensor to the target in microseconds.
  rangefinder = IS_LOW                     ' make trigger 0-1-0
  PULSOUT rangefinder, RF_TRIGGER          ' activate sensor
  PULSIN rangefinder, IS_HIGH, distance    ' measure echo pulse
  distance = distance */ RF_SCALE          ' convert to uS
  distance = distance / 2                  ' remove return trip
  distance = distance ** RF_RAW_TO_IN      ' convert to inches
  RETURN


'====( calibrate_accelerometer )==============================
calibrate_accelerometer:
  ' read x_force and y_force while we're
  '  (supposedly) standing still, to calibrate
  '  accelerometer
  DEBUG "Set robot down flat for calibration.", CR
  PAUSE 5000

  GOSUB read_x_force
  x_force_calibration = x_force
  GOSUB read_y_force
  y_force_calibration = y_force

  RETURN


'====( read_x_force )========================================
read_x_force:
  PULSIN accel_x, IS_HIGH, x_force
  x_force = x_force */ ACCEL_SCALE
  x_force = ((x_force / 10) - 500) * 8     ' convert to g-force (1/1000ths)
  x_force = x_force - x_force_calibration  ' subtract the calibration value
  RETURN


'====( read_y_force )========================================
read_y_force:
  PULSIN accel_y, IS_HIGH, y_force
  y_force = y_force */ ACCEL_SCALE
  y_force = ((y_force / 10) - 500) * 8     ' convert to g-force (1/1000ths)
  y_force = y_force - y_force_calibration  ' subtract the calibration value
  RETURN
 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Hey, I like the youtube-describtion much better:

Herpe is a curious-yet-skittish-puppy-bot. What he's SUPPOSED to do is seek you out and drive up to you, but if he gets too close, he backs away in fear. In reality, he's a bit of an idiot.

:D

Quote from http://www.youtube.com/watch?v=yoCfVi8NU_0

Nice and clean code too! 

 


We don't stop playing because we grow old, we grow old because we stop playing.

Looks like a really good learning, and "I'm gonna get you next time"-project. And I know these ;)

Thank you for sharing!

/ Frits

Nice Robot!

Love how it backs away as if its fearful! 

Thanks :) It did provide several hours of fun when my friends came over and would take turns chasing it around the house. It was fun to build, and of course very exciting to see how much I could accomplish with that little circuit board (this was my first work with a microcontroller). I think eventually I want to build another quick little robot that can have this same kind of fearful, skitterish behavior, darting away from things that frighten it.