Code: /****************************************************************************
* HiTechnic Accelerometer Device Driver *
* *
* Based upon the RobotC "NXT Hitechnic Accelerometer.c" *
* file and the "NXT Compas Sensor Driver.c" files. *
* Note: the SensorValue variable is not big enough to store the *
* 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. *
* *
* Author: David C Trotz Jr. *
* dtrotzjr@yahoo.com *
* Revision: 1.0 *
* Date: 06/16/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;
} HTAccel_Output;
typedef struct
{
int x;
int y;
int z;
int time;
} TAccelValue;
typedef enum
{ subTypeNone = 0,
subTypeHiTechnicCompass = 1,
subTypeHiTechnicRcxIR = 2,
subTypeHiTechnicAccel = 3,
subTypeMindsensorsCompass = 20,
subTypeMindsensorsRcxIR = 21,
subTypeMindsensorsPSX = 22,
subTypeMindsensorsMotorMux = 22,
} TSensorSubTypes;
typedef enum
{
stateNotAccel,
stateAccelInitSend,
stateAccelInitWaitReply,
stateAccelPollSend,
stateAccelPollWaitReply,
} TAccelState;
// Driver Status values
TAccelValue AccelValue[4];
TAccelState nDriverState[4] =
{ stateNotAccel, stateNotAccel, stateNotAccel, stateNotAccel };
tSensors kAccelPort = S1;
task AccelerometerDeviceDriver()
{
if(SensorSubType[kAccelPort] != subTypeHiTechnicAccel)
return; // Not initialized!
nSchedulePriority = 200;
HTAccel_Output sOutput;
byte nReplyBytes[6];
while(true)
{
switch(nDriverState[kAccelPort])
{
case stateNotAccel:
nDriverState[kAccelPort] = stateAccelInitSend;
SensorType[kAccelPort] = sensorI2CCustomFast;
// Fall through to allow initialization...
case stateAccelInitSend:
// Nothing to do for this sensor.
nDriverState = stateAccelInitWaitReply;
break;
case stateAccelInitWaitReply:
switch(nI2CStatus[kAccelPort])
{
case NO_ERR:
nDriverState[kAccelPort] = stateAccelPollSend;
break;
case STAT_COMM_PENDING:
break;
default:
case ERR_COMM_BUS_ERR:
nDriverState[kAccelPort] = stateAccelInitSend;
break;
}
break;
case stateAccelPollSend:
if(nI2CStatus[kAccelPort] == STAT_COMM_PENDING)
break;
sOutput.nMsgSize = 2;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x42;
sendI2CMsg(kAccelPort, sOutput.nMsgSize, 6);
nDriverState[kAccelPort] = stateAccelPollWaitReply;
break;
case stateAccelPollWaitReply:
switch(nI2CStatus[kAccelPort])
{
case NO_ERR:
readI2CReply(S1, nReplyBytes[0], 6);
AccelValue[kAccelPort].x = ((int)nReplyBytes[0] << 2) + (0xFF & nReplyBytes[3]);
AccelValue[kAccelPort].y = ((int)nReplyBytes[1] << 2) + (0xFF & nReplyBytes[4]);
AccelValue[kAccelPort].z = ((int)nReplyBytes[2] << 2) + (0xFF & nReplyBytes[5]);
AccelValue[kAccelPort].time = time10[T1];
nDriverState[kAccelPort] = stateAccelPollSend;
break;
case STAT_COMM_PENDING:
break;
default:
case ERR_COMM_BUS_ERR:
nDriverState[kAccelPort] = stateAccelInitSend;
break;
}
break;
}
// In theory we could poll every 10 ms but this gave me some weird values.
wait1Msec(50);
}
}
void initAccelerometer(tSensors kPort)
{
kAccelPort = kPort;
SensorType[kAccelPort] = sensorI2CCustomStd;
SensorSubType[kAccelPort] = subTypeHiTechnicAccel;
StartTask(AccelerometerDeviceDriver);
}
task main()
{
tSensors kAccel = S1;
initAccelerometer(kAccel);
time1[T1] = 0;
while(true)
{
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);
}
}
| 
