BrickOS Stall Detector
I decided to use my new rotation sensor code to try a different
approach to collision detection for robots: stall
detection. One thread samples the output of a rotation sensor
periodically. If it gathers enough samples that shows no movement on
the rotation sensor it decides that a stall has occurred.
I've gone a step further from the previous two projects and
implemented a thread based solution. The rotation sensor is checked
periodically by the detect_stall() thread. The
main() function starts the thread executing using the
execi() BrickOS call. You can initialize as many threads as you
wish using execi() and they will all start running when the
tm_start() kernel call starts the task scheduler.
The detect_stall() function loops forever and monitors the
rotation sensor value every STALL_INTERVAL microseconds. We
must observe STALL_SAMPLES iterations through the loop with
no change in the rotation sensor reading before we decide that a stall
has occurred. If the value has changed since the last sample point
then we reset the sampleCount counter to 0 to indicate that
we are not stalled.
If the sampleCount counter reaches STALL_SAMPLES
then we make a beep sound and call stop_and_reverse() to
reverse the motor direction.
HINTS
- The sensitivity of the stall detector can be tuned by changing the
value of STALL_SAMPLES. A greater value will decrease the
sensitivity to stalls.
- It is best to include a rubber-band drive step into a robot using
this code. If the drive train from the motor to the wheels is all gear
driven then the gears will be stressed when the robot runs into an
obstacle. The rubber bands provide some slippage in the drive
mechanism to allow the stall sensor to trigger.
Click here to download the source code.
/*
* stall.c
*
* Detect a robot stalling when it hits an object
* Constantly monitors the rotation sensor as a background thread and
* if it detects a stall then beep and stop and reverse the motors
*
* Assumes: rotation sensor on sensor port 1
* motor output on port A
*
* Mark Crosbie 4/8/00 mark@mastincrosbie.com
*/
#include
#include
#include
#include
#include
#include
#define ROTSENSOR SENSOR_1
#define DRIVEMOTOR MOTOR_A
#define FORW 0
#define REV 1
/* how many samples should we take before calling a stall? */
#define STALL_SAMPLES 3
/* time interval between each sample point in ms */
#define STALL_INTERVAL 100
int detect_stall(int argc, char **argv);
void stop_and_reverse(void);
int dir; /* motor direction */
int main(int argc, char *argv[]) {
int stallpid;
/* we start out going forward */
dir = FORW;
/* initialize the sensor */
ds_init();
ds_active(&ROTSENSOR);
/* set the sensor's count to 0 */
ds_rotation_set(&ROTSENSOR, 0);
/* ok, start sensor processing */
ds_rotation_on(&ROTSENSOR);
/* declare the thread to detect a stall */
stallpid = execi(detect_stall, 0, (char **)NULL, PRIO_NORMAL,
DEFAULT_STACK_SIZE);
/* start task manager processing */
tm_start();
motor_a_speed(MAX_SPEED);
motor_a_dir(fwd);
cputs("fwwd ");
while(1) {
}
}
/*
* Name:
* detect_stall
*
* Description:
* Monitor the rotation sensor and detect if it has stopped
* changing. If it has, stop the motors and reverse direction
*
*
* Parameters:
* argc: (in) ignored
* argv: (in) ignored
*
* Returns:
* 0 if no change in value
* 1 if value of ROTSENSOR has changed since last poll
*/
int detect_stall(int argc, char **argv) {
int value;
int sampleCount;
value = ROTATION_1; /* take an initial reading */
sampleCount = 0; /* no samples have been taken */
/* we live forever, or at least until the main loop kills us :-) */
while(1) {
msleep(STALL_INTERVAL);
if(value != ROTATION_1) {
/* change is good. It means we're moving, so update value and
* reset sampleCount
*/
value = ROTATION_1;
sampleCount = 0;
} else {
/* ok, time to take samples. If this is the last sample of the
* set, then we call it a stall and reverse out of here
*/
if(sampleCount == STALL_SAMPLES) {
/* beep so we know it */
dsound_system(DSOUND_BEEP);
stop_and_reverse();
value = ROTATION_1;
sampleCount = 0;
} else {
/* update the sampleCount */
sampleCount++;
}
}
}
}
/*
* Name:
* stop_and_reverse
*
* Description:
* Stop the motor and reverse direction
*
* Parameters:
*
* Returns:
*/
void stop_and_reverse(void) {
motor_a_dir(brake);
/* toggle direction */
if(dir == FORW) {
dir = REV;
motor_a_dir(rev);
cputs("rev ");
} else {
dir = FORW;
motor_a_dir(fwd);
cputs("fwwd ");
}
motor_a_speed(MAX_SPEED);
}
|
