ROBOTC.net forums
http://robotc.net/forums/

HiTechnic Accelerometer Driver
http://robotc.net/forums/viewtopic.php?f=41&t=204
Page 1 of 1

Author:  dtrotzjr [ Sat Jun 16, 2007 11:35 pm ]
Post subject:  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

Author:  tag115 [ Tue Dec 02, 2008 9:50 pm ]
Post subject:  Re: HiTechnic Accelerometer Driver

hi i am new too ineed to know how to program the accelerometer sensor

Author:  spaceballs [ Wed Dec 03, 2008 5:14 am ]
Post subject:  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
}

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/