Subject:
|
IRPD sensors and LegOS (long)
|
Newsgroups:
|
lugnet.robotics.rcx.legos
|
Date:
|
Wed, 30 Aug 2000 07:17:16 GMT
|
Reply-To:
|
nospam-dcchen@pacbell.netSTOPSPAM-nospam
|
Viewed:
|
1367 times
|
| |
|
|
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 makes available at www.flash.net/~sevcik
They are a little steep at $29.00 apiece, but you get Left/Front/Right
proximity detection using just one input port.
I like them so much I bought 2 of em!!! I gotta tell you, LegOS really
works well with this. I was getting fair results with sending IR Pings
via the IR Port and using the built in Light Sensors, but this is
working
much better.
Here is the code I cobbed together. Note, this program currently keeps
track of the last 10 turns and distances travelled, but doesn't do
anything
with them. Working on a robot that can re-trace it's steps based on
this
one. The code's not great, but feel free to play with it.
Dave
/*
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
*
* The contents of this file are subject to the Mozilla Public License
* Version 1.0 (the "License"); you may not use this file except in
* compliance with the License. You may obtain a copy of the License at
* http://www.mozilla.org/MPL/
*
* Software distributed under the License is distributed on an "AS IS"
* basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See
the
* License for the specific language governing rights and limitations
* under the License.
*
* The Original Code is legOS .lx code, released August 30, 2000.
*
* The Initial Developer of the Original Code is David C. Chen.
*
*/
#include <stdlib.h>
#include <dsensor.h>
#include <dmotor.h>
#include <dlcd.h>
#include <conio.h>
#include <unistd.h>
#include <sys/tm.h>
#define SpeedA 220 /* Adjusts for difference in torques between your */
#define SpeedC 255 /* A and C motors */
#define TurnTime 680 /* turns for this many msec */
#define BackupTime 400 /* backs up this many msec */
#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;
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 ShowStuff()
{
while(1)
{
if((BATTERY/0x0016)<700) /* Rotation sensor gets flaky if < 7v */
dlcd_show(LCD_BATTERY_X); /* Show low battery Indicator if < 7v
*/
lcd_int(ROTATION_1); /* Show Rotation count on the LCD */
cputc_hex_0(Direction); /* Show Current Heading */
msleep(100);
}
}
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()
{
motor_a_speed(SpeedA);
motor_c_speed(SpeedC);
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 = 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 is on Lt */
{ ++Direction; /* Right turn adds one */
motor_a_dir(fwd); /* ...so turn to the Right */
}
msleep(TurnTime); /* For TurnTime msec.
*/
Direction &= 0x07; /* Keep Direction within range 0-7 */
}
}
int main()
{
task_index = 0;
StartTask(&StopTask);
srandom(sys_time); /* Set a Random Number Seed */
ds_active(&SENSOR_2); /* Power the IRDP Sensor on Port 2 */
ds_active(&SENSOR_1); /* Power up Rotation Sensor Port 1 */
ds_rotation_on(&SENSOR_1); /* Initialize sensor */
msleep(1000);
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); /* Go! */
tm_start();
return(0);
}
--
dcchen@pacbell.net
|
|
Message has 2 Replies:
 | | Re: IRPD sensors and LegOS (long)
|
| Since IRPD doesn't use the upper half of the sensor range (above 512), it looks like it could easily be multiplexed with a touch sensor. Has anyone tried this? (I would try it myself, but I haven't bought an IRPD [yet!].) (...) -- ___...___ $50,000 (...) (24 years ago, 30-Aug-00, to lugnet.robotics.rcx.legos)
| | | Re: IRPD sensors and LegOS (long)
|
| 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 (...) (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
|
|
|
|
