Mindstorms 3rd Party ROBOTC Drivers RobotC
[Home] [Download] [Submit a bug/suggestion] [ROBOTC Forums] [Blog] [Support this project]

hitechnic-angle.h

Go to the documentation of this file.
00001 /*!@addtogroup HiTechnic
00002  * @{
00003  * @defgroup htang Angle Sensor
00004  * HiTechnic Angle Sensor
00005  * @{
00006  */
00007 
00008 /*
00009  * $Id: hitechnic-angle.h 133 2013-03-10 15:15:38Z xander $
00010  */
00011 
00012 #ifndef __HTANG_H__
00013 #define __HTANG_H__
00014 /** \file hitechnic-angle.h
00015  * \brief HiTechnic Angle Sensor driver
00016  *
00017  * hitechnic-angle.h provides an API for the HiTechnic Angle Sensor.
00018  *
00019  * Changelog:
00020  * - 0.1: Initial release
00021  * - 0.2: Replaced array structs with typedefs
00022  *
00023  *
00024  * Credits:
00025  * - Big thanks to HiTechnic for providing me with the hardware necessary to write and test this.
00026  *
00027  * License: You may use this code as you wish, provided you give credit where its due.
00028  *
00029  * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. 
00030 
00031  * \author Xander Soldaat (xander_at_botbench.com)
00032  * \date 20 February 2011
00033  * \version 0.2
00034  * \example hitechnic-angle-test1.c
00035  * \example hitechnic-angle-SMUX-test1.c
00036  */
00037 
00038 #pragma systemFile
00039 
00040 #ifndef __COMMON_H__
00041 #include "common.h"
00042 #endif
00043 
00044 #define HTANG_I2C_ADDR         0x02      /*!< HTCS2 I2C device address */
00045 #define HTANG_CMD_REG          0x41      /*!< Command register */
00046 #define HTANG_OFFSET           0x42      /*!< Offset for data registers */
00047 
00048 // Values contained by registers in active mode
00049 #define HTANG_ANG2             0x00      /*!< Current angle (2 deg increments) */
00050 #define HTANG_ANG1             0x01      /*!< Current angle (1 deg adder) */
00051 #define HTANG_ACC_ANG_B4       0x02      /*!< 32 bit accumulated angle 4th byte */
00052 #define HTANG_ACC_ANG_B3       0x03      /*!< 32 bit accumulated angle 3rd byte */
00053 #define HTANG_ACC_ANG_B2       0x04      /*!< 32 bit accumulated angle 2nd byte */
00054 #define HTANG_ACC_ANG_B1       0x05      /*!< 32 bit accumulated angle 1st byte */
00055 #define HTANG_RPM_H            0x06      /*!< 16 bit rpms, high byte */
00056 #define HTANG_RPM_L            0x07      /*!< 16 bit rpms, low byte */
00057 
00058 // Various commands
00059 #define HTANG_CMD_MEASURE      0x00      /*!< Normal angle measurement mode */
00060 #define HTANG_CMD_RST_ANG      0x43      /*!< Resets 0 position to current shaft angle, non-volatile setting */
00061 #define HTANG_CMD_RST_ACC_ANG  0x52      /*!< Resets the accumulated angle */
00062 
00063 int HTANGreadAngle(tSensors link);
00064 long HTANGreadAccumulatedAngle(tSensors link);
00065 int HTANGreadRPM(tSensors link);
00066 bool HTANGresetAngle(tSensors link);
00067 bool HTANGresetAccumulatedAngle(tSensors link);
00068 bool _HTANGsendCommand(tSensors link, byte command);
00069 
00070 #ifdef __HTSMUX_SUPPORT__
00071 int HTANGreadAngle(tMUXSensor muxsensor);
00072 long HTANGreadAccumulatedAngle(tMUXSensor muxsensor);
00073 int HTANGreadRPM(tMUXSensor muxsensor);
00074 
00075 tConfigParams HTANG_config = {HTSMUX_CHAN_I2C, 8, 0x02, 0x42};  /*!< Array to hold SMUX config data for sensor */
00076 #endif // __HTSMUX_SUPPORT__
00077 
00078 tByteArray HTANG_I2CRequest;             /*!< Array to hold I2C command data */
00079 tByteArray HTANG_I2CReply;               /*!< Array to hold I2C reply data */
00080 
00081 
00082 /**
00083  * Return the current angle
00084  * @param link the HTANG port number
00085  * @return current angle or -1 if an error occurred.
00086  */
00087 int HTANGreadAngle(tSensors link) {
00088   memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00089 
00090   HTANG_I2CRequest[0] = 2;                         // Message size
00091   HTANG_I2CRequest[1] = HTANG_I2C_ADDR;            // I2C Address
00092   HTANG_I2CRequest[2] = HTANG_OFFSET + HTANG_ANG2; // Start Current angle
00093 
00094   if (!writeI2C(link, HTANG_I2CRequest, HTANG_I2CReply, 2))
00095     return -1;
00096 
00097   return (HTANG_I2CReply[0] * 2) + HTANG_I2CReply[1];
00098 }
00099 
00100 
00101 /**
00102  * Return the current angle
00103  * @param muxsensor the SMUX sensor port number
00104  * @return current angle or -1 if an error occurred.
00105  */
00106 #ifdef __HTSMUX_SUPPORT__
00107 int HTANGreadAngle(tMUXSensor muxsensor) {
00108         memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00109 
00110   if (HTSMUXSensorTypes[muxsensor] != HTSMUXSensorCustom)
00111     HTSMUXconfigChannel(muxsensor, HTANG_config);
00112 
00113   if (!HTSMUXreadPort(muxsensor, HTANG_I2CReply, 2, HTANG_ANG2)) {
00114     return -1;
00115   }
00116 
00117   return (HTANG_I2CReply[0] * 2) + HTANG_I2CReply[1];
00118 }
00119 #endif // __HTSMUX_SUPPORT__
00120 
00121 
00122 /**
00123  * Return the accumulated angle (signed 32 bit value)
00124  * @param link the HTANG port number
00125  * @return current angle or -1 if an error occurred.
00126  */
00127 long HTANGreadAccumulatedAngle(tSensors link) {
00128   memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00129 
00130   HTANG_I2CRequest[0] = 2;                                // Message size
00131   HTANG_I2CRequest[1] = HTANG_I2C_ADDR;                   // I2C Address
00132   HTANG_I2CRequest[2] = HTANG_OFFSET + HTANG_ACC_ANG_B4;  // Start accumulated angle
00133 
00134   if (!writeI2C(link, HTANG_I2CRequest, HTANG_I2CReply, 4))
00135     return -1;
00136 
00137   return (HTANG_I2CReply[0] << 24) +
00138          (HTANG_I2CReply[1] << 16) +
00139          (HTANG_I2CReply[2] <<  8) +
00140           HTANG_I2CReply[3];
00141 }
00142 
00143 
00144 /**
00145  * Return the accumulated angle (signed 32 bit value)
00146  * @param muxsensor the SMUX sensor port number
00147  * @return current angle or -1 if an error occurred.
00148  */
00149 #ifdef __HTSMUX_SUPPORT__
00150 long HTANGreadAccumulatedAngle(tMUXSensor muxsensor) {
00151         memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00152 
00153   if (HTSMUXSensorTypes[muxsensor] != HTSMUXSensorCustom)
00154     HTSMUXconfigChannel(muxsensor, HTANG_config);
00155 
00156   if (!HTSMUXreadPort(muxsensor, HTANG_I2CReply, 4, HTANG_ACC_ANG_B4)) {
00157     return -1;
00158   }
00159 
00160   return (HTANG_I2CReply[0] << 24) +
00161          (HTANG_I2CReply[1] << 16) +
00162          (HTANG_I2CReply[2] <<  8) +
00163           HTANG_I2CReply[3];
00164 }
00165 #endif // __HTSMUX_SUPPORT__
00166 
00167 
00168 /**
00169  * Return the rpm that the shaft is currently rotating at
00170  * @param link the HTANG port number
00171  * @return the current rpm of the shaft or -1 if an error occurred.
00172  */
00173 int HTANGreadRPM(tSensors link) {
00174   memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00175 
00176   HTANG_I2CRequest[0] = 2;                           // Message size
00177   HTANG_I2CRequest[1] = HTANG_I2C_ADDR;              // I2C Address
00178   HTANG_I2CRequest[2] = HTANG_OFFSET + HTANG_RPM_H;  // Start of rpm
00179 
00180   if (!writeI2C(link, HTANG_I2CRequest, HTANG_I2CReply, 2))
00181     return -1;
00182 
00183   return (HTANG_I2CReply[0] <<  8) +
00184           HTANG_I2CReply[1];
00185 }
00186 
00187 
00188 /**
00189  * Return the rpm that the shaft is currently rotating at
00190  * @param muxsensor the SMUX sensor port number
00191  * @return the current rpm of the shaft or -1 if an error occurred.
00192  */
00193 #ifdef __HTSMUX_SUPPORT__
00194 int HTANGreadRPM(tMUXSensor muxsensor) {
00195   memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00196 
00197   if (HTSMUXSensorTypes[muxsensor] != HTSMUXSensorCustom)
00198     HTSMUXconfigChannel(muxsensor, HTANG_config);
00199 
00200   if (!HTSMUXreadPort(muxsensor, HTANG_I2CReply, 2, HTANG_RPM_H)) {
00201     return -1;
00202   }
00203 
00204   // return HTANG_I2CReply[0] * 2 + HTANG_I2CReply[1];
00205 
00206   return (HTANG_I2CReply[0] <<  8) +
00207           HTANG_I2CReply[1];
00208 }
00209 #endif // __HTSMUX_SUPPORT__
00210 
00211 
00212 /**
00213  * Reset the 0 position to the current shaft angle.<br>
00214  * Note: this will also reset the accumulated angle counter
00215  * @param link the HTANG port number
00216  * @return true if no error occured, false if it did
00217  */
00218 bool HTANGresetAngle(tSensors link) {
00219   return _HTANGsendCommand(link, HTANG_CMD_RST_ANG);
00220 }
00221 
00222 
00223 /**
00224  * Reset the accumulated angle
00225  * @param link the HTANG port number
00226  * @return true if no error occured, false if it did
00227  */
00228 bool HTANGresetAccumulatedAngle(tSensors link) {
00229   return _HTANGsendCommand(link, HTANG_CMD_RST_ACC_ANG);
00230 }
00231 
00232 
00233 /**
00234  * Send a command to the sensor
00235  *
00236  * Note: this is an internal function and should not be called directly.
00237  * @param link the HTANG port number
00238  * @param command the command to be sent to the sensor
00239  * @return true if no error occured, false if it did
00240  */
00241 bool _HTANGsendCommand(tSensors link, byte command) {
00242   memset(HTANG_I2CRequest, 0, sizeof(tByteArray));
00243 
00244   HTANG_I2CRequest[0] = 3;              // Message size
00245   HTANG_I2CRequest[1] = HTANG_I2C_ADDR; // I2C Address
00246   HTANG_I2CRequest[2] = HTANG_CMD_REG;  // Command register
00247   HTANG_I2CRequest[3] = command;        // Command to be sent
00248 
00249   return writeI2C(link, HTANG_I2CRequest);
00250 }
00251 
00252 
00253 #endif // __HTANG_H__
00254 
00255  /*
00256  * $Id: hitechnic-angle.h 133 2013-03-10 15:15:38Z xander $
00257  */
00258 /* @} */
00259 /* @} */