Subject:
|
Re: Short term memory ablut sensor input???
|
Newsgroups:
|
lugnet.robotics.rcx
|
Date:
|
Thu, 21 Sep 2000 21:49:21 GMT
|
Viewed:
|
1595 times
|
| |
|
|
Can you remember a touch sensor reading? This way by using only one light
sensor as an irpd, I can detect which way to turn to avoid the obstacle.
In lugnet.robotics.rcx, David Chen writes:
> Agreed, LegOS is the way to go. Here is a LegOS program I wrote for
> a robot that keeps track of its heading in one of 8, increments (45
> degrees intervals, numbered 0 to 7 heading clockwise).
>
> It keeps track of the last 10 segments in it's travels (each defined
> with a heading and a distance travelled). It is able to retrace
> it's steps if you want it to. The Obstacle detection is for the
> very nice made IRPD DUal Proximity sensor made by TechnoStuff, but
> you can easily change the code to use touch sensors if you want.
>
> Right now, it only uses the short term memory for getting out of
> corners, but you can use it for heading out to a destination and finding
> your way back. Unfortunately, Lego gearing has lots of slop and
> Odometry
> errors add up pretty fast, especially since it is difficult to turn
> exactly 45 degrees each time. A compass sensor would be nice.
>
> Feel free to modify and use the code,
>
> Dave
>
> /*
> IRPD Auto Rover for LegOS 0.2.4
> 8/30/00
> By David Chen dcchen@pacbell.net
>
> This code is GPL
>
> 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 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 220 /* A and C motors */
>
> #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 4
> #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((sys_time - RunTime)/100); /* Show Runtime */
> cputc_hex_4(TCount); /* Show Collision Count */
> 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;
> }
> }
>
> /*
> ------------------------------------------------------------------------------ • ------
> */
>
> 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 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(((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);
> 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_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 */
> RunTime = sys_time;
> TCount = 0;
>
> StartTask(&ShowStuff);
> StartTask(&Move);
>
> tm_start();
>
> return(0);
> }
>
>
> Carsten Müller wrote:
> >
> > Matthew Miller wrote:
> > >
> > > Carsten Müller <spirou@mobile.de> wrote:
> > > > That's not quite right. It is a little bit weard but you can use the
> > > > counter to detect different states :)
> > >
> > > That's true. You can't remember something like a light-sensor reading,
> > > though. My thinking is: if you're at the point where you want to be doing
> > > stuff like this, you're probably ready to graduate to a better system!
> > never mind. I've done this once, to prove, that you can build a little
> > robot, which won't get stuck in any kind
> > of corner. Changing strategie, if it detects a corner, in which it was
> > stuck. an programm it only with this kid programming language :)
> >
> > for all other projects i use legOs.
> >
> > Carsten
|
|
Message has 1 Reply:
 | | Re: Short term memory ablut sensor input???
|
| (...) The program doesn't actually record in the Vectors array any information about where it saw an obstacle. It just records which direction the robot went and how far (according to the rotation sensor) during the last leg of the journey. Each (...) (24 years ago, 22-Sep-00, to lugnet.robotics.rcx)
|
Message is in Reply To:
| | Re: Short term memory ablut sensor input???
|
| Agreed, LegOS is the way to go. Here is a LegOS program I wrote for a robot that keeps track of its heading in one of 8, increments (45 degrees intervals, numbered 0 to 7 heading clockwise). It keeps track of the last 10 segments in it's travels (...) (24 years ago, 21-Sep-00, to lugnet.robotics.rcx)
|
8 Messages in This Thread:
 
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
This Message and its Replies on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
