Subject:
|
Re: IRPD sensors and LegOS (long)
|
Newsgroups:
|
lugnet.robotics.rcx.legos
|
Date:
|
Wed, 4 Oct 2000 09:09:52 GMT
|
Viewed:
|
1460 times
|
| |
|
|
OK, the IRPD rover just smarter.
I had an error in the comments in the previously posted version,
references
in the initial comments about the Right Side Motor being hooked up to
Output B were incorrect, it is actually hooked up to Output C.
This version uses Dual Rotation sensors on Input 1 and Input 3 to
monitor
the motor speeds and an additional Task named "Correct" keeps the two
motors
running closer to the same speed to improve straight line travel. I'll
add
an additional routine that monitors turns to ensure that the 45 degree
turns
are more accurate in the future.
Give it a spin, if you don't have an IRPD sensor on Sensor 2, perhaps
you can
use a touch sensor and change the code in the Interrupt routine
"Obstacle"
accordingly. The recent post about using stock Lego light bricks as
resistors
suddenly opens up the posibility of multiplexing two touch sensors into
one input port using stock lego parts. See post#12654
(http://news.lugnet.com/robotics/?n=12654)
This thing is almost getting precise enough to navigate by dead
reckoning.
Dave
Code follows:
/*
IRPD Auto Rover for LegOS 0.2.4
8/30/00
By David Chen dcchen@pacbell.net
Input 1<=Rotation Sensor (Left Track)
Input 2<=IRPD Dual Proximity Sensor
Input 3<=Rotation Sensor (Right Track)
OutputA=>Left Wheel/Drive
OutputC=>Right Wheel/Drive
IRPD readings as follows, your mileage may very according to your
battery levels.
Using the built in macro for Sensor scaling LIGHT_2
No Obstacle = 190 +/- 2 ONone
Obstacle Left = 35 +/- 2 OLeft
Obstacle Right = 112 +/- 2 ORight
Obstacle Ahead = 473 +/- 3 OBoth
Backs up slightly and turns away from obstacles to its left and right.
Keeps track of the last MAX_VECTORS number of Headings and Distances
travelled
Retraces the last two vectors/segments travelled if there is an
obstacle ahead
If it senses it is stuck in one place ( it does > CollLimit number of
turns
within TimeLimit msec), retraces the last vector before it got stuck.
*/
#include <stdlib.h>
#include <dsensor.h>
#include <dmotor.h>
#include <dlcd.h>
#include <conio.h>
#include <unistd.h>
#include <sys/tm.h>
#define TimeLimit 9000
#define CollLimit 4
#define MAX_VECTORS 10
#define SpeedA 255 /* Adjusts for difference in torques between your */
#define SpeedC 255 /* A and C motors */
#define CorrSpeed 126 /* The slower speed to correct for one motor being
stronger */
#define Slop 0 /* Allowed difference in wheel travel before correction
occurs */
#define TurnTime 620 /* Turns for this many msec for a 45 degree turn
*/
/* Adjust this number based on your robot's drivetrain */
#define BackupTime 400 /* Backs up this many msec when an obstacle is
encountered */
/* Ensures front end of the robot is clear before turning */
#define OLeft 1
#define OBoth 2
#define ORight 3
#define ONone 0
#define FwdAll {motor_a_dir(fwd); motor_c_dir(fwd);}
#define RevAll {motor_a_dir(rev); motor_c_dir(rev);}
#define LeftTurn {motor_a_dir(rev); motor_c_dir(fwd);}
#define RightTurn {motor_a_dir(fwd); motor_c_dir(rev);}
#define StopAll {motor_a_dir(brake); motor_c_dir(brake);}
#define MAX_TASKS 6
#define abs(x) (((x) < 0) ? -(x) : (x))
struct Vector /* Each segment of the robot's travel is recorded as a
Bearing */
{ /* and Distance travelled. */
char Bearing;
int Distance;
};
pid_t pid[MAX_TASKS]; /* Array to keep track of task pid's so they can
be cleaned up */
struct Vector Path[MAX_VECTORS]; /* Rotating que of recent Vectors
travelled */
int task_index, ODir, Index, TCount;
long RunTime; /* Tracks time since non-obstructed travel */
char Direction; /* Current Heading */
/*
------------------------------------------------------------------------------------
*/
/* These two tasks executes and keeps track of PID's and cleanly kill
all tasks upon */
/* termination of the program by the user hitting the RUN key. */
void StartTask(int (*code_start)())
{
pid[task_index++] = execi(code_start, 0, NULL, 0, DEFAULT_STACK_SIZE);
}
int StopTask()
{
int i;
msleep(200);
while (getchar()!= KEY_RUN);
for (i = 0; i < task_index; i++)
kill(pid[i]);
ds_passive(&SENSOR_1);
ds_passive(&SENSOR_2);
return 0;
}
/*
------------------------------------------------------------------------------------
*/
void TurnTo(char X) /* Turns Robot to heading X */
{
int T, c;
X &= 0x07;
T = X - Direction; /* Find the shortest direction */
/* to turn CW or CCW */
if(T < -4) T += 8;
if(T > 4) T -= 8;
if(T < 0) {
LeftTurn;
T *= -1;}
else
RightTurn;
for(c = 0; c < T; ++c) /* Turn T number of 45 degree turns */
msleep(TurnTime);
StopAll;
msleep(200);
Direction = X; /* Update Direction */
}
/*
------------------------------------------------------------------------------------
*/
int ShowStuff() /* Resets Collision counter and shows variable
contents */
{ /* Can use this to debug and show other Variables/States */
while(1)
{
if(((sys_time - RunTime) > TimeLimit) && (TCount < CollLimit))
{ /* Haven't had to turn from an obstacle in awhile so reset */
RunTime = sys_time;
TCount = 0;
}
if((BATTERY/0x0016)<700) /* Rotation sensor gets flaky if Battery <
7v */
dlcd_show(LCD_BATTERY_X); /* Show low battery Indicator if drops
to < 7v */
lcd_int(ROTATION_1 - ROTATION_3); /* Show difference in A and C
wheel travel */
cputc_hex_0(Direction); /* Show current heading */
switch(ODir) /* Show which direction an Obstacle has been detected
*/
{
case OBoth:
dlcd_hide(LCD_A_SELECT);dlcd_hide(LCD_C_SELECT);dlcd_show(LCD_B_SELECT);
break;
case OLeft:
dlcd_hide(LCD_C_SELECT);dlcd_hide(LCD_B_SELECT);dlcd_show(LCD_A_SELECT);
break;
case ORight:
dlcd_hide(LCD_B_SELECT);dlcd_hide(LCD_A_SELECT);dlcd_show(LCD_C_SELECT);
break;
case ONone:
dlcd_hide(LCD_A_SELECT);dlcd_hide(LCD_B_SELECT);dlcd_hide(LCD_C_SELECT);
break;
}
msleep(100);
}
}
/*
------------------------------------------------------------------------------------
*/
void Retrace(int Steps)
{
int T, S;
for(S=Steps;S > -1; --S)
{
T = (Path[Index].Bearing - 4) & 0x07; /* Reverse Last Heading */
TurnTo(T);
ds_rotation_set(&SENSOR_1,0); /* Zero Odometer */
FwdAll;
while (abs(ROTATION_1) < Path[Index].Distance) /* Go until distance
is retraced */
if(Index < 0) Index = MAX_VECTORS;
Index--; /* Go to next previous segment */
StopAll;
}
}
/*
------------------------------------------------------------------------------------
*/
int Correct() /* Using dual Rotation Sensors on Input Ports 1 & 3 to
correct for */
/* Speed differences of Motors on Output Ports A & C resectively */
{
while(1)
{
while(((ROTATION_1 - ROTATION_3) < -Slop)&&(ODir==ONone))
motor_c_speed(CorrSpeed);
while(((ROTATION_1 - ROTATION_3) > Slop)&&(ODir==ONone))
motor_a_speed(CorrSpeed);
motor_a_speed(SpeedA);
motor_c_speed(SpeedC);
}
}
/*
------------------------------------------------------------------------------------
*/
wakeup_t Obstacle(wakeup_t t) /* Check for IR Proximity detection */
{
ODir = LIGHT_2; /* Get a reading */
if(LIGHT_2 > 250)
ODir = OBoth;
else if(LIGHT_2 < 45)
ODir = OLeft;
else if (LIGHT_2 < 150)
ODir = ORight;
else ODir = ONone;
return (ODir); /* Returns location of Obstacle if any */
}
/*
------------------------------------------------------------------------------------
*/
int Move()
{
while(1)
{
ds_rotation_set(&SENSOR_1,0); /* Clear the Last distance readings */
ds_rotation_set(&SENSOR_3,0);
FwdAll;
wait_event(&Obstacle,0); /* wait until we detect an obstacle */
StopAll;
msleep(200);
RevAll;
msleep(BackupTime);
Path[Index].Bearing = Direction; /* Record Heading and Distance */
Path[Index].Distance = abs(ROTATION_1); /* Travelled on this leg */
(++Index > MAX_VECTORS ? Index = 0 : Index);
if(((sys_time - RunTime) > TimeLimit) && (TCount > CollLimit))
{ /* If > CollLimit number of avoidances occur in */
Index -= CollLimit;
if(Index < 0) Index += MAX_VECTORS;
Retrace(1); /* TimeLimit amount of time, start retracing at */
RunTime = sys_time; /* the point before got stuck (likely a
corner) */
TCount = 0;
}
else
{
switch(ODir)
{
case OBoth: /* An object straight ahead */
msleep(BackupTime); /* Pick a random direction to turn */
++TCount;
Retrace(2);
break;
case ORight: /* Obstacle on the Rt */
--Direction; /* Left turn subtracts one */
motor_c_dir(fwd); /* ...so turn to the Left */
++TCount;
break;
case OLeft: /* But if the obstacle on Lt */
++Direction; /* Right turn adds one */
motor_a_dir(fwd); /* ...so turn to the Right */
++TCount;
break;
}
msleep(TurnTime); /* For TurnTime msec. */
StopAll;
msleep(200);
ODir = ONone;
Direction &= 0x07; /* Keep Direction within range 0-7 */
}
}
return(0);
}
/*
------------------------------------------------------------------------------------
*/
int main()
{
task_index = 0; /* Start Program End/Task Killer */
StartTask(&StopTask);
srandom(sys_time); /* Set a Random Number Seed */
ds_active(&SENSOR_1); /* Power up Rotation Sensor Port 1 */
ds_rotation_on(&SENSOR_1); /* Initialize sensor */
ds_active(&SENSOR_3); /* Power up Rotation Sensor Port 3 */
ds_rotation_on(&SENSOR_3); /* Initialize sensor */
ds_active(&SENSOR_2); /* Power the IRDP Sensor on Port 2 */
msleep(500); /* Let readings stablize after power up */
motor_a_speed(SpeedA); /* Set Motor Speeds, play with SpeedA */
motor_c_speed(SpeedC); /* and SpeedC to match unequal motors. */
Index = 0; /* Initialize index to Path array */
Direction = 0; /* Call starting direction 0, increasing */
/* as you go clockwise by 1 / 45 degrees */
ODir = 0;
RunTime = sys_time;
TCount = 0;
StartTask(&ShowStuff); /* Show debugging information on the LCD */
StartTask(&Correct); /* Correct for speed differences in the */
/* A/C Differential drive train */
StartTask(&Move); /* Go!! */
tm_start();
return(0);
}
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
