Subject:
|
Re: IRPD sensors and LegOS (long)
|
Newsgroups:
|
lugnet.robotics.rcx.legos
|
Date:
|
Wed, 30 Aug 2000 20:57:23 GMT
|
Reply-To:
|
nospam-dcchen@pacbell(IHateSpam).net-nospam
|
Viewed:
|
1336 times
|
| |
|
|
Well, I worked on it a bit and added two routines:
TurnTo(int New_Direction) => Figures the quickest way to turn from the
current
Direction to the new one.
Retrace(int N) => Retraces the last N number of legs of it's journey.
Change the #define LEGS to whatever number of Legs you want the robot to
go
out on and then return...ie #define LEGS 5, the robot will go out
avoiding
obstacles, after the 5th turn, it will backtrack to where it began.
However, due to the inherent slop in the Gears and stuff, the robot
doesn't
quite end up back where it started. It typically ends up a few inches
off to
one side, this error builds as the overall journey gets longer.
Next step, to use dual rotation sensors to more accurately track
Odometry and
reduce errors. What we really need is some of compass sensor to track
true
headings...sigh.
Code could be cleaned up somewhat and optimized. Need a generalized
Obstacle
reporting task that could be easily modified for different sensor
types. Next
version will try to create an internal map and navigate intelligently
around,
hopefully. A rather large array will be needed though. Assuming you
use a grid
represented by a 2d array of Char. If you have each grid size about
that of 1
robot length (8 inches). To cover a 10 foot x 10 foot room you would
need approx
15 bytes x 15 bytes = 225 bytes, not too bad. Going to 20 ft x 20 ft
pushes this
to 900 bytes. Might be doable...
Dave
Code as follows, enjoy and cut up at will:
/*
IRPD Rover for LegOS 0.2.4
8/27/00
By David Chen dcchen@pacbell.net
Input 1<=Rotation Sensor
Input 2<=IRPD Dual Proximity Sensor
OutputA=>Left Wheel/Drive
OutputB=>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
Obstacle Left = 35 +/- 2
Obstacle Right = 112 +/- 2
Obstacle Ahead = 473 +/- 3
*/
#include <stdlib.h>
#include <dsensor.h>
#include <dmotor.h>
#include <dlcd.h>
#include <conio.h>
#include <unistd.h>
#include <sys/tm.h>
#define LEGS 3 /* Set how many Segments of travel the robot takes */
/* before returning by tracing it's steps. */
#define SpeedA 255 /* Adjusts for difference in torques between your */
#define SpeedC 255 /* A and C motors */
#define TurnTime 615 /* 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 */
#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 10
#define MAX_VECTORS 10
struct Vector
{
char Bearing;
int Distance;
};
pid_t pid[MAX_TASKS];
struct Vector Path[MAX_VECTORS];
int task_index, ODir, Index;
char Direction, Goal;
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 StartTask(int (*code_start)())
{
pid[task_index++] = execi(code_start, 0, NULL, 0, DEFAULT_STACK_SIZE);
}
int abs(int x)
{
if(x<0) x*= -1;
return(x);
}
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() /* Shows Distance Travelled and Current Heading */
{ /* Can use this to debug and show other Variables/States */
while(1)
{
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); /* Show Rotation count on the LCD */
cputc_hex_0(Direction); /* Show Current Heading */
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;
}
}
wakeup_t Obstacle(wakeup_t t) /* Check for IR Proximity detection */
{
ODir = LIGHT_2; /* Get a reading */
return ((ODir < 150) || (ODir > 250)); /* Returns 1 in the event of an
Obstacle */
}
int Move()
{
while(Index < LEGS) /* Run around until LEGS number of direction
changes occur */
{
ds_rotation_set(&SENSOR_1,0); /* Clear the Last distance reading */
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(ODir > 250) /* An object straight ahead */
{ msleep(BackupTime); /* Pick a random direction to turn */
if(random() & 0x01)
ODir = 30;
else
ODir = 120;
}
if(ODir > 70) /* Obstacle on the Rt */
{ --Direction; /* Left turn subtracts one */
motor_c_dir(fwd); /* ...so turn to the Left
*/
}
else /* But if the obstacle on Lt */
{ ++Direction; /* Right turn adds one */
motor_a_dir(fwd); /* ...so turn to the Right */
}
msleep(TurnTime); /* For TurnTime msec.
*/
StopAll;
msleep(200);
Direction &= 0x07; /* Keep Direction within range 0-7 */
}
Retrace(LEGS); /* Retrace your steps and see if you get back to the
same spot */
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_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 */
StartTask(&ShowStuff);
StartTask(&Move);
tm_start();
/* Test TurnTo routine stuff, you can use this by commenting out the
StartTask(&Move);
line and uncommenting this section of code to calibrate your robot to
find workable
settings for TurnTime, SpeedA and SpeedC
Goal = 0;
while(1)
{
for(Goal= 0; Goal < 9; ++Goal)
{
sleep(3);
TurnTo(Goal);
}
for(Goal= 7; Goal > 0; --Goal)
{
sleep(3);
TurnTo(Goal);
}
}
*/
return(0);
}
|
|
Message is in Reply To:
 | | IRPD sensors and LegOS (long)
|
| Well, after much frustration and a great deal of help from the very wise Rossz, I was finally able to get up and running w/ DJGPP and LegOS 0.2.4 The first thing I did was write a program to use with Pete Sevcik's IRPD proximity detectors that he (...) (24 years ago, 30-Aug-00, to lugnet.robotics.rcx.legos)
|
3 Messages in This Thread:
 
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
