Subject:
|
Proximity detector routines
|
Newsgroups:
|
lugnet.robotics.rcx.legos
|
Date:
|
Thu, 20 Jan 2000 13:18:20 GMT
|
Viewed:
|
1464 times
|
| |
|
|
Hello,
i've written a proximity sensor for legOS 0.2.3
I hope, it'll be usefull to someone.
Carsten
-------------------SNIP HERE ------------------------------
/********************************************
* Filename: ProximityLib.c *
* *
* Proximity Driver for legOS 0.2.3 *
* the LightSensor is connected PORT 2 *
* Carsten Müller 2 Jan 2000 *
********************************************/
#include <unistd.h>
#include <dsensor.h>
#include <lnp.h>
#define LIGHT_SENSOR SENSOR_2
#define PRXTHRESSHOLD 0x30 // difflight value
#define TRANSMITTING 1
#define IDLE 0
volatile int ClockPhase; // Indicator of Transmitter
(TRANSMITTING | IDLE)
volatile int ProximityCounter; // Reflection Counter (max
0x0a)
volatile int Detecting; // Detection Indicator
static int transmitter(int argc,char **argv){
while(1){
S_SR&=~(SSR_TRANS_EMPTY | SSR_TRANS_END); // clean up Flags
S_CR|=SCR_TRANSMIT; // enable Transmitter
S_TDR = 0x00; // transmit as many
lightpulses as possible
ClockPhase=TRANSMITTING; // Indicate transmitting
msleep(3); // wait a little while
ClockPhase=IDLE; // Indicate Idle
msleep(6); // and wait again
}
}
static int count_Reflections(int argc,char ** argv){
static int ThisLight=0,LastLight=0,DiffLight=0;
while(1){
ProximityCounter=0;
while(ClockPhase==IDLE){} // we have to wait until there is a
transmission
while(ClockPhase == TRANSMITTING){
Detecting = 1;
LastLight = ThisLight;
ThisLight = LIGHT_SENSOR>>7;
DiffLight = LastLight;
DiffLight -= ThisLight;
if(DiffLight > PRXTHRESSHOLD){
ProximityCounter++;
}
}
Detecting=0;
}
}
pid_t Transmitter=0,Counter=0;
pid_t getTransmitterProcess(){
return Transmitter;
}
pid_t getCounterProcess(){
return Counter;
}
int init_Proximity(){
// Activate the Lightsensor
ds_active(&LIGHT_SENSOR);
// Set Baudrate
S_BRR=B2400;
// Set IR transmitter to longrange
*((char*)&PORT4) &=~1;
// Check if there is a Transmitter Process running
if(Transmitter!=0){
kill(Transmitter);
Transmitter=0;
}
// Start Transmitter Process
Transmitter = execi(transmitter,0,NULL,10,512); // fork a new
Process
if(!Transmitter){
return 0;
}
// Check if there is a Counter Process running
if(Counter!=0){
kill(Counter);
Counter=0;
}
// Start Counter Process
Counter = execi(count_Reflections,0,NULL,10,512); // and the second
one
if(!Counter){
kill(Transmitter);
return 0;
}
// Return success
return 1;
}
-------------------------------------------------------------------------------------
/***************************************
* Filename: ProximityLib.h *
* Headerfile for ProximityLib.c *
***************************************/
extern volatile int ProximityCounter;
extern volatile int ClockPhase;
extern volatile int Detecting;
// the Transmitter Process
// transmitts as much IR signals as possible
extern int transmitter(int agrc,char **argv);
// The Counter Process
// counts Light Peeks during transmission phase of the transmitter
process
// the counts are stored in ProximityCounter
extern int count_Reflections(int argc,char ** argv);
// The initializer
// starts the transmitter and the counter process
extern int init_Proximity();
// returns the procID of the transmitter process
extern pid_t getTransmitterProcess();
// returns the procID of the counter process
extern pid_t getCounterProcess();
-------------------------------------------------------------------------------------------
/******************************************
* Filename: proximity.c *
* simple first test for proximity tester *
* *
* the Lightsensor is on Port 2 *
* first stage only reads data from port *
* and display the value in Hex on the *
* LCD. *
******************************************/
#include <conio.h>
#include <unistd.h>
#include <dsensor.h>
#include <dsound.h>
#include <lnp.h>
#include "ProximityLib.h"
static const note_t ping[]={
{PITCH_D4,0},{PITCH_END,0}};
int main(int argc, char **argv){
init_Proximity();
while(1){
if(ProximityCounter!=0){
dsound_play(ping);
wait_event(dsound_finished,0);
}
}
return 0;
}
---------------------------------------------------------------------------------
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
