View unanswered posts | View active topics It is currently Thu Jul 31, 2014 11:20 pm






Reply to topic  [ 3 posts ] 
HiTechnic Accelerometer Driver 
Author Message
Rookie

Joined: Sat Jun 16, 2007 6:24 pm
Posts: 14
Post HiTechnic Accelerometer Driver
Hi all,
I am new here. I recently got into the Mindstorms and must say I love RobotC for programming them.

I am in the process of writing an advanced state estimator using the wheel encoders, accelerometer, compass and hopefully a gyro.

I noticed there is no official Accelerometer driver got the HiTechnic device so I am donating mine to the community. It is based upon other examples and appears to work fine. Let me know if anyone has any problems. I plan to integrate the compass driver soon as well, so that only one task is used to read from the sensors.

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);
  }
}



Enjoy!
-- David Trotz


Sat Jun 16, 2007 11:35 pm
Profile
Rookie

Joined: Tue Dec 02, 2008 9:31 pm
Posts: 2
Post Re: HiTechnic Accelerometer Driver
hi i am new too ineed to know how to program the accelerometer sensor


Tue Dec 02, 2008 9:50 pm
Profile
Rookie

Joined: Sat Apr 19, 2008 11:51 am
Posts: 43
Post Re: HiTechnic Accelerometer Driver
hello,
to my opinion it's no good to poll the sensor values by an endless task.
You just have a limited number of tasks in RobotC and one usually already needs every single one of them, and above this it's a waste of cpu power.

So usually one polls and assigns a sensor value just in the moment when you need to have it, and it's useless to poll the values when you don't have to know about it.

The best way is to do it like
Code:
#define Touch_port S1
#define Compass_port S2
#define Accel_port S3

long my_var;

SensorType[Touch_port]=SensorTouch;
SensorType[Compass_port]=SensorHTCompass;
SensorType[Accel_port]=SensorHTAccelerometer; // <<<  to be accessed by YOUR interface routines!

my_var=SensorValue(Touch_port);
if (my_var==...) {
  // do sth specific
}
my_var=SensorValue(Compass_port);
if (my_var==...) {
  // do sth specific
}
my_var=SensorValue(Accel_port); // <<<  to be accessed by YOUR interface routines!
if (my_var==...) { // MAYBE x,y,z encoded in a long integer value
  // do sth specific
}


Wed Dec 03, 2008 5:14 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 3 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

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.