to mark as completed Herpe
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.
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
Try to get a certain distance from an object
- CPU: Basic Stamp 2
- Sensors / input devices: PING))) sonar, Parallax accelerometer
- Target environment: indoor