|
Page 1 of 1
|
[ 4 posts ] |
|
| Author |
Message |
|
dtrotzjr
Rookie
Joined: Sat Jun 16, 2007 6:24 pm
Posts: 14
|
 Sensor Drivers... Hi All,
I have updated my I2C Sensor Drivers to include more sensors. Up to this release 1.2 I have implemented
1) HiTechnic Accelerometer
2) HiTechnic Compass
3) Mindsensors Long Range Distance Finder, Dist-NxL
Should be good for all Mindsensor Dist-Nx sensors
4) Hi-technic Color Sensor (Basic operation)
I am open to suggestions or updates to include any sensors you may have.
Enjoy!!!
P.S. You will note that some of the code is directly taken from the RobotC Compass source code. I did not want people to think I am taking credit for Dick's hard work, I am simply building upon it.
 | | | | Code: /****************************************************************************
* I2C Device Drivers *
* *
* *
* Author: David C Trotz Jr. *
* dtrotzjr[NOSPAM] _AT_ yahoo _DOT_ com *
* Revision: 1.2 *
* Changelog: *
* dct -- Added HiTechnic Color Sensor *
* *
* Date: 07/22/2007 *
***************************************************************************/
/****************************************************************************
* *
* *
* This driver is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This driver is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with ReadingPlanner; if not, write to the Free Software *
* Foundation, Inc. *
* 59 Temple Place, Suite 330, *
* Boston, MA 02111-1307 USA *
* *
* *
****************************************************************************/
#pragma platform(NXT)
// Supporting Data Structures
typedef struct
{
byte nMsgSize;
byte nDeviceAddress;
byte nLocationPtr;
byte nData;
} TI2C_Output;
/****************************************************************************
* Note: the SensorValue variable is not big enough to store the *
* Accelerometer x y z values, so I made a AccelValue array to mimic the *
* SensorValue array behavior with the exception that x,y,z are *
* members of the TAccelValue type. I also added a time stamp to *
* the type so that some correlation can be made with other sensors *
* in hopes of future state estimation attempts by me. *
****************************************************************************/
typedef struct{
int x;
int y;
int z;
unsigned int time;
} TAccelValue;
TAccelValue AccelValue[4];
typedef enum {
subTypeNone = 0,
subTypeHiTechnicCompass = 1, // HiTechnic Compass Sensor
subTypeHiTechnicRcxIR = 2, // HiTechnic RcxIR
subTypeHiTechnicAccel = 3, // HiTechnic Accelerometer
subTypeHiTechnicColor = 4, // HiTechnic Color Sensor
subTypeMindsensorsCompass = 20,
subTypeMindsensorsRcxIR = 21,
subTypeMindsensorsPSX = 22,
subTypeMindsensorsMotorMux = 22,
subTypeMindsensorsDistNx = 23, // Mindsensor's Infrared Distance Sensor
} TSensorSubTypes;
typedef enum {
stateNotI2C,
stateI2CInitSend,
stateI2CInitWaitReply,
stateI2CPollSend,
stateI2CPollWaitReply,
} TI2CState;
// Driver Status values
bool driverStarted = false;
TI2CState nDriverState[4] =
{ stateNotI2C, stateNotI2C, stateNotI2C, stateNotI2C };
tSensors nDDPortIndex;
bool deviceDriverHTAccel()
{
TI2C_Output sOutput;
byte nReplyBytes[6];
switch(nDriverState[nDDPortIndex])
{
default:
return false;
case stateNotI2C:
nDriverState[nDDPortIndex] = stateI2CInitSend;
SensorType[nDDPortIndex] = sensorI2CCustomFast;
// Fall through to allow initialization...
case stateI2CInitSend:
// Nothing to do for this sensor. So we skip a stateI2InitWaitReply...
nDriverState[nDDPortIndex] = stateI2CPollSend;
return false;
case stateI2CInitWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
case stateI2CPollSend:
if(nI2CStatus[nDDPortIndex] == STAT_COMM_PENDING)
return false;
sOutput.nMsgSize = 2;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x42;
sendI2CMsg(nDDPortIndex, sOutput.nMsgSize, 6);
nDriverState[nDDPortIndex] = stateI2CPollWaitReply;
return false;
case stateI2CPollWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
readI2CReply(S1, nReplyBytes[0], 6);
AccelValue[nDDPortIndex].x = ((int)nReplyBytes[0] << 2) + (0xFF & nReplyBytes[3]);
AccelValue[nDDPortIndex].y = ((int)nReplyBytes[1] << 2) + (0xFF & nReplyBytes[4]);
AccelValue[nDDPortIndex].z = ((int)nReplyBytes[2] << 2) + (0xFF & nReplyBytes[5]);
AccelValue[nDDPortIndex].time = time1[T1];
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
}
}
bool deviceDriverDistNx()
{
TI2C_Output sOutput;
byte nReplyBytes[2];
switch(nDriverState[nDDPortIndex])
{
default:
return false;
case stateNotI2C:
nDriverState[nDDPortIndex] = stateI2CInitSend;
SensorType[nDDPortIndex] = sensorI2CCustomFast;
// Fall through to allow initialization...
case stateI2CInitSend:
// Nothing to do for this sensor. So we skip a stateI2InitWaitReply...
nDriverState[nDDPortIndex] = stateI2CPollSend;
return false;
case stateI2CInitWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
case stateI2CPollSend:
if(nI2CStatus[nDDPortIndex] == STAT_COMM_PENDING)
return false;
sOutput.nMsgSize = 2;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x42;
sendI2CMsg(nDDPortIndex, sOutput.nMsgSize, 2);
nDriverState[nDDPortIndex] = stateI2CPollWaitReply;
return false;
case stateI2CPollWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
readI2CReply(nDDPortIndex, nReplyBytes[0], 2);
SensorValue[nDDPortIndex] = (((((int)nReplyBytes[1]) & 0x00FF) << 8) | (((int)nReplyBytes[0]) & 0x00FF));
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
}
}
bool deviceDriverHiTechnicColor()
{
TI2C_Output sOutput;
byte nReplyBytes[1];
switch(nDriverState[nDDPortIndex])
{
default:
return false;
case stateNotI2C:
nDriverState[nDDPortIndex] = stateI2CInitSend;
SensorType[nDDPortIndex] = sensorI2CCustomFast;
// Fall through to allow initialization...
case stateI2CInitSend:
// Nothing to do for this sensor. So we skip a stateI2InitWaitReply...
nDriverState[nDDPortIndex] = stateI2CPollSend;
return false;
case stateI2CInitWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
case stateI2CPollSend:
if(nI2CStatus[nDDPortIndex] == STAT_COMM_PENDING)
return false;
sOutput.nMsgSize = 2;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x42; // Request Color Number
sendI2CMsg(nDDPortIndex, sOutput.nMsgSize, 1);
nDriverState[nDDPortIndex] = stateI2CPollWaitReply;
return false;
case stateI2CPollWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
readI2CReply(nDDPortIndex, nReplyBytes[0], 1);
SensorValue[nDDPortIndex] = nReplyBytes[0];
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
}
}
//////////////////////////////////////////////////////////////////////////////////////////
//
// Hitechnic Digital Compass Device Driver
//
// The device driver for a Hitechnic Digital Compass.
//
// Driver returns 'true' when a single polling cycle has been completed. It
// returns false to indicate that the current cycle needs more time.
//
// The driver is written as a state machine. It has three main stages:
//
// 1. One time setup of the sensor. State is also entered when the
// sensor needs to be reset because of a communications error.
//
// 2. Send a poll request to the compass to read the current value.
//
// 3. Wait for the poll to complete and read the results. Then go to state
// two.
//
//////////////////////////////////////////////////////////////////////////////////////////
bool deviceDriverHTCompass()
{
TI2C_Output sOutput;
byte nReplyBytes[2];
switch (nDriverState[nDDPortIndex])
{
default:
return false;
case stateNotI2C:
nDriverState[nDDPortIndex] = stateI2CInitSend;
SensorType[nDDPortIndex] = sensorI2CCustomFast;
case stateI2CInitSend:
if (nI2CStatus[nDDPortIndex] == STAT_COMM_PENDING)
return false;
// Flush the input buffer in case there are any bytes left from previous I2C messages.
while (nI2CBytesReady[nDDPortIndex] > 0)
readI2CReply(nDDPortIndex, nReplyBytes[0], 1);
sOutput.nMsgSize = 3;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x41;
sOutput.nData = 0x00;
sendI2CMsg(nDDPortIndex, sOutput.nMsgSize, 0);
nDriverState[nDDPortIndex] = stateI2CInitWaitReply;
return false;
case stateI2CInitWaitReply:
switch (nI2CStatus[nDDPortIndex])
{
case NO_ERR:
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
// Keep waiting for reply. I2C messaging is not complete
return false;
default:
case ERR_COMM_BUS_ERR:
// re-initialize sensor. An I2C messaging error occurred.
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
case stateI2CPollSend:
if (nI2CStatus[nDDPortIndex] == STAT_COMM_PENDING)
return false;
sOutput.nMsgSize = 2;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x44;
sendI2CMsg(nDDPortIndex, sOutput.nMsgSize, 2);
nDriverState[nDDPortIndex] = stateI2CPollWaitReply;
return false;
case stateI2CPollWaitReply:
switch (nI2CStatus[nDDPortIndex])
{
case NO_ERR:
readI2CReply(nDDPortIndex, nReplyBytes[0], 2);
SensorValue[nDDPortIndex] = (0xFF & nReplyBytes[1]) * 256 + (uword) (0xFF & nReplyBytes[0]);
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
// Keep waiting for reply. I2C messaging is not complete
return false;
default:
case ERR_COMM_BUS_ERR:
// re-initialize sensor. An I2C messaging error occurred.
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
}
}
task taskI2CDeviceDrivers()
{
nxtDisplayTextLine(0, "Driver Started");
//
// Give this task high priority to ensure that it runs frequently
//
// NOTE: this task makes frequent use of 'wait' functions to ensure that it does not
// hog the available CPU cycles. If you don't do this, then the higher priority
// task will consume all the CPU cycles and lower priority tasks will not run!
//
nSchedulePriority = 200;
driverStarted = true;
//
// Loop forever, checking one sensor about every 5 milliseconds to see if it is a
// Hitechnic sensor
//
while (true){
for (nDDPortIndex = S1; nDDPortIndex <= S4; ++nDDPortIndex){
switch (SensorSubType[nDDPortIndex])
{
case subTypeHiTechnicCompass:
while (!deviceDriverHTCompass()){
wait1Msec(10);
}
break;
case subTypeHiTechnicAccel:
while (!deviceDriverHTAccel()){
wait1Msec(10);
}
break;
case subTypeMindsensorsDistNx:
while (!deviceDriverDistNx()){
wait1Msec(10);
}
break;
case subTypeHiTechnicColor:
while (!deviceDriverHiTechnicColor()){
wait1Msec(10);
}
break;
default:
break;
}
}
wait1Msec(10);
}
return;
}
void initHTAccelerometer(tSensors kPort)
{
AccelValue[kPort].x = 0;
AccelValue[kPort].y = 0;
AccelValue[kPort].z = 0;
AccelValue[kPort].time = 0;
time1[T1] = 0;
SensorType[kPort] = sensorI2CCustomFast;
SensorSubType[kPort] = subTypeHiTechnicAccel;
if(!driverStarted)
StartTask(taskI2CDeviceDrivers);
}
void initCompassSensor(tSensors kPort)
{
SensorType[kPort] = sensorI2CCustomStd;
SensorSubType[kPort] = subTypeHiTechnicCompass;
if(!driverStarted)
StartTask(taskI2CDeviceDrivers);
}
void initDistNxSensor(tSensors kPort)
{
SensorType[kPort] = sensorI2CCustomStd;
SensorSubType[kPort] = subTypeMindsensorsDistNx;
if(!driverStarted)
StartTask(taskI2CDeviceDrivers);
}
void initHiTechnicColor(tSensors kPort)
{
SensorType[kPort] = sensorI2CCustomStd;
SensorSubType[kPort] = subTypeHiTechnicColor;
if(!driverStarted)
StartTask(taskI2CDeviceDrivers);
}
task main()
{
SensorSubType[S1] = subTypeNone;
SensorSubType[S2] = subTypeNone;
SensorSubType[S3] = subTypeNone;
SensorSubType[S4] = subTypeNone;
int nCycles = 0;
int nCompassHeading = 0;
int nDistance = 0;
int nColor = 0;
tSensors kAccel = S1;
initHTAccelerometer(kAccel);
tSensors kCompass = S2;
initCompassSensor(kCompass);
tSensors kDistNxL = S3;
initDistNxSensor(kDistNxL);
tSensors kHTColor = S4;
initHiTechnicColor(kHTColor);
while(true)
{
++nCycles;
nCompassHeading = SensorValue[kCompass];
nDistance = SensorValue[kDistNxL];
nColor = SensorValue[kHTColor];
nxtDisplayTextLine(0, "Cycle: %d", nCycles);
// For corresponding color values refer to:
// http://www.hitechnic.com/index.html?lang=en-us&target=d18.html
nxtDisplayTextLine(1, "Color %d", nColor);
nxtDisplayTextLine(2, "Heading: %d", (short) nCompassHeading);
nxtDisplayTextLine(3, "Dist: %d", nDistance);
nxtDisplayTextLine(4, "x-val: %d", AccelValue[kAccel].x);
nxtDisplayTextLine(5, "y-val: %d", AccelValue[kAccel].y);
nxtDisplayTextLine(6, "z-val: %d", AccelValue[kAccel].z);
nxtDisplayTextLine(7, "time: %d", AccelValue[kAccel].time);
wait1Msec(100);
}
}
| | | | |
Last edited by dtrotzjr on Tue Jul 31, 2007 9:29 am, edited 2 times in total.
|
| Sun Jun 24, 2007 11:44 pm |
|
|
|
JamesD
Novice
Joined: Sun Feb 04, 2007 12:48 am
Posts: 69
Location: Australia
|
Hi David,
Thank you very much for your awesome post above. It is a huge help in opening up use of those sensors to the amateur coding community.
As coding at the driver level appears way out of my league, at the moment, I was hoping you or other able programmers may be able to help me and others make better use of your & Dick's work.
I've looked on the web and on the mindsensors site and sample drivers for DIST-Nx using RobotC don't appear to exist. (Mindsensors indicate they can be found in the sample files but I can't see them). As I only have one of the sensors in your code I've basically tried to strip out the code that doesn't appear to be related to the mindsensors DIST-Nx IR sensor. My problem is two fold.
1) I think I've taken out something that caused it to stop sampling after a very short period. But I'm not sure how to fix it.
2) I'm not sure what adjustments are needed to customise the driver for the medium distance sensor rather than the long distance sensor. Is this a simple process?
Thanks
James
What I'd like to be able to do is work out how to incorporate the drivers into other existing programs. My butchered version of your code is as follows: (please don't cringe too much) 
| | | | Code:
#pragma platform(NXT)
// Supporting Data Structures
typedef struct
{
byte nMsgSize;
byte nDeviceAddress;
byte nLocationPtr;
byte nData;
} TI2C_Output;
typedef enum {
subTypeNone = 0,
subTypeMindsensorsCompass = 20,
subTypeMindsensorsDistNxL = 23, // Long Range Infrared Distance Sensor
} TSensorSubTypes;
typedef enum {
stateNotI2C,
stateI2CInitSend,
stateI2CInitWaitReply,
stateI2CPollSend,
stateI2CPollWaitReply,
} TI2CState;
// Driver Status values
bool driverStarted = false;
TI2CState nDriverState[4] =
{ stateNotI2C, stateNotI2C, stateNotI2C, stateNotI2C };
tSensors nDDPortIndex;
bool deviceDriverDistNxL()
{
TI2C_Output sOutput;
byte nReplyBytes[2];
switch(nDriverState[nDDPortIndex])
{
default:
return false;
case stateNotI2C:
nDriverState[nDDPortIndex] = stateI2CInitSend;
SensorType[nDDPortIndex] = sensorI2CCustomFast;
// Fall through to allow initialization...
case stateI2CInitSend:
// Nothing to do for this sensor. So we skip a stateI2InitWaitReply...
nDriverState[nDDPortIndex] = stateI2CPollSend;
return false;
case stateI2CInitWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
case stateI2CPollSend:
if(nI2CStatus[nDDPortIndex] == STAT_COMM_PENDING)
return false;
sOutput.nMsgSize = 2;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x42;
sendI2CMsg(nDDPortIndex, sOutput.nMsgSize, 2);
nDriverState[nDDPortIndex] = stateI2CPollWaitReply;
return false;
case stateI2CPollWaitReply:
switch(nI2CStatus[nDDPortIndex])
{
case NO_ERR:
readI2CReply(nDDPortIndex, nReplyBytes[0], 2);
SensorValue[nDDPortIndex] = (((((int)nReplyBytes[1]) & 0x00FF) << 8) | (((int)nReplyBytes[0]) & 0x00FF));
nDriverState[nDDPortIndex] = stateI2CPollSend;
return true;
case STAT_COMM_PENDING:
return false;
default:
case ERR_COMM_BUS_ERR:
nDriverState[nDDPortIndex] = stateI2CInitSend;
return true;
}
break;
}
}
task taskI2CDeviceDrivers()
{
nxtDisplayTextLine(0, "Driver Started");
//
// Give this task high priority to ensure that it runs frequently
//
// NOTE: this task makes frequent use of 'wait' functions to ensure that it does not
// hog the available CPU cycles. If you don't do this, then the higher priority
// task will consume all the CPU cycles and lower priority tasks will not run!
//
nSchedulePriority = 200;
driverStarted = true;
//
// Loop forever, checking one sensor about every 5 milliseconds to see if it is a
// Hitechnic sensor
//
while (true){
for (nDDPortIndex = S1; nDDPortIndex <= S4; ++nDDPortIndex){
switch (SensorSubType[nDDPortIndex])
{
case subTypeMindsensorsDistNxL:
while (!deviceDriverDistNxL()){
wait1Msec(10);
}
break;
default:
break;
}
}
wait1Msec(10);
}
return;
}
void initDistNxLSensor(tSensors kPort)
{
SensorType[kPort] = sensorI2CCustomStd;
SensorSubType[kPort] = subTypeMindsensorsDistNxL;
if(!driverStarted)
StartTask(taskI2CDeviceDrivers);
}
task main()
{
int nCycles = 0;
int nDistance = 0;
tSensors kDistNxL = S4;
initDistNxLSensor(kDistNxL);
while(true)
{
++nCycles;
nDistance = SensorValue[kDistNxL];
nxtDisplayTextLine(1, "Cycle: %d", nCycles);
nxtDisplayTextLine(3, "Dist: %d", nDistance);
wait1Msec(100);
}
}
| | | | |
|
| Tue Jul 17, 2007 9:24 am |
|
|
|
dtrotzjr
Rookie
Joined: Sat Jun 16, 2007 6:24 pm
Posts: 14
|
At first glance everything appears to be in order. My driver should work fine with the other two IR Range finders from Mindsensors. Have you tried it with my driver just to be sure it does work? Also you might want to keep in mind that you can safely include my whole driver into any app you want to use the driver for. The unused ports/drivers are never invoked unless you do so. This will make it easier for you in the long run as I may find a bug in my code and post an update (in fact if you look closely between my original submission and the all inclusive driver you will see I fixed two bugs) then you can simply replace the file.
But I applaud your desire to slim down the code to do only what you want it to do, I would do the same  its just a coders bad habit. So in an effort to help you tell me what you mean when you say it stops sampling, have you seen if the device driver task is still running? put a few print statements in the driver task to see where it might be dropping out.
Let me know what you find. I don't have RobotC's firmware on my robot as I have been using nXc lately but I plan on putting it on to write the HiTechnic Color sensor driver for anther thread here.
Keep up the good work 
|
| Wed Jul 18, 2007 1:30 am |
|
|
|
docilio
Expert
Joined: Sun Sep 09, 2007 10:12 am
Posts: 116
|
Excelent Post...
I am trying it now...
How could we work with Color Sensor, with the 3 colorS?
Any program of IR Seeker Sensor?
Thanks all
|
| Sun Sep 09, 2007 10:33 am |
|
|
|
|
Page 1 of 1
|
[ 4 posts ] |
|
Who is online |
Users browsing this forum: No registered users and 1 guest |
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum
|
|
