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

common.h

Go to the documentation of this file.
00001 /*!@addtogroup common_includes
00002  * @{
00003  * @defgroup common Main common include
00004  * Commonly used functions used by drivers
00005  * @{
00006  */
00007 
00008 /*
00009  * $Id: common.h 134 2013-03-12 18:15:17Z xander $
00010  */
00011 
00012 /** \file common.h
00013  * \brief Commonly used functions used by drivers.
00014  *
00015  * common.h provides a number of frequently used functions that are useful for writing
00016  * drivers.
00017  * License: You may use this code as you wish, provided you give credit where its due.
00018  *
00019  * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER.
00020 
00021  *
00022  * Changelog:
00023  * - 0.1: Initial release
00024  * - 0.2: Added version check to issue error when compiling with RobotC < 1.46
00025  * - 0.2: Added __COMMON_H_DEBUG__ to enable/disable sounds when an I2C error occurs
00026  * - 0.2: Removed bool waitForI2CBus(tSensors link, bool silent)
00027  * - 0.3: clearI2CError() added to make writeI2C more robust, I2C bus errors are now handled
00028  *        better.
00029  * - 0.4: Added HiTechnic SMUX functions
00030  * - 0.5: Added clip function (Tom Roach)
00031  * - 0.6: clearI2CBus is now conditionally compiled into the FW.  Only RobotC < 1.57 needs it.
00032  * - 0.7: ubyteToInt(byte byteVal) modified, works better with 1.57+
00033  * - 0.8: ubyte used for arrays for firmware version 770 and higher<br>
00034  *        added support for new colour sensor<br>
00035  *        added better handling for when sensor is not configured properly
00036  * - 0.9: added bool HTSMUXsetMode(tSensors link, byte channel, byte mode) prototype<br>
00037  *        added int HTSMUXreadAnalogue(tMUXSensor muxsensor)<br>
00038  *        added HTSMUXSensorType HTSMUXreadSensorType(tMUXSensor muxsensor)<br>
00039  *        added bool HTSMUXreadPort(tMUXSensor muxsensor, tByteArray &result, int numbytes, int offset)<br>
00040  *        added bool HTSMUXreadPort(tMUXSensor muxsensor, tByteArray &result, int numbytes)<br>
00041  *        added bool HTSMUXsetMode(tMUXSensor muxsensor, byte mode)<br>
00042  *        added bool HTSMUXsetAnalogueActive(tMUXSensor muxsensor)<br>
00043  *        added bool HTSMUXsetAnalogueInactive(tMUXSensor muxsensor)<br>
00044  *        corrected function description for HTSMUXSensorType()
00045  * - 0.10: Removed unnecessary read from HTSMUXsendCommand()
00046  * - 0.11: Added long uByteToLong(byte a1, byte a2, byte a3, byte a4);
00047  * - 0.12: Added HTSMUXreadPowerStatus(tSensors link)<br>
00048  *         Added int round(float fl)
00049  * - 0.13: Added motor mux types and data structs
00050  * - 0.14: Added check for digital sensors to prevent conflict with built-in drivers\n
00051  *         Changed clearI2CError to take ubyte for address, thanks Aswin
00052  * - 0.15: Removed motor mux and sensor mux functions and types out
00053  * - 0.16: Added max() and min() functions by Mike Henning, Max Bareiss
00054  *
00055  * \author Xander Soldaat (xander_at_botbench.com)
00056  * \date 27 April 2011
00057  * \version 0.16
00058  */
00059 
00060 #pragma systemFile
00061 
00062 #ifndef __COMMON_H__
00063 #define __COMMON_H__
00064 
00065 #undef __COMMON_H_DEBUG__
00066 //#define __COMMON_H_DEBUG__
00067 
00068 /*!< define this as 0 to remove the check  */
00069 #ifndef __COMMON_H_SENSOR_CHECK__
00070 #define __COMMON_H_SENSOR_CHECK__ 1
00071 #else
00072 #warn "sensor checking disabled, I hope you know what you are doing!"
00073 #endif
00074 
00075 #include "firmwareVersion.h"
00076 #if (kRobotCVersionNumeric < 359)
00077 #error "These drivers are only supported on RobotC version 3.59 or higher"
00078 #endif
00079 
00080 #ifndef MAX_ARR_SIZE
00081 /**
00082  * Maximum buffer size for byte_array, can be overridden in your own program.
00083  * It's 17 bytes big because the max I2C buffer size is 16, plus 1 byte to denote
00084  * packet length.
00085  */
00086 #define MAX_ARR_SIZE 17
00087 #endif
00088 
00089 
00090 /**
00091  * This define returns the smaller of the two numbers
00092  */
00093 #define min2(a, b) (a < b ? a : b)
00094 
00095 
00096 /**
00097  * This define returns the smallest of the three numbers
00098  */
00099 #define min3(a, b, c) (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c)
00100 
00101 
00102 /**
00103  * This function returns the bigger of the two numbers
00104  */
00105 #define max2(a, b) (a > b ? a : b)
00106 
00107 
00108 /**
00109  * This function returns the biggest of the three numbers
00110  */
00111 #define max3(a, b, c) (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c)
00112 
00113 
00114 /**
00115  * Returns x if it is between min and max. If outside the range,
00116  * it returns min or max.
00117  */
00118 #define clip(a, b, c) min2(c, max2(b, a))
00119 
00120 
00121 /**
00122  * Array of bytes as a struct, this is a work around for RobotC's inability to pass an array to
00123  * a function.
00124  */
00125 typedef ubyte tByteArray[MAX_ARR_SIZE];
00126 typedef sbyte tsByteArray[MAX_ARR_SIZE];
00127 typedef ubyte tMassiveArray[128];             /*!< 128 byte array for very large blobs of data */
00128 typedef ubyte tHugeByteArray[64];             /*!< 64 byte array for very large blobs of data */
00129 typedef ubyte tBigByteArray[32];              /*!< 32 byte array for large blobs of data */
00130 typedef ubyte tIPaddr[4];                     /*!< Struct for holding an IP address */
00131 
00132 /**
00133  * Array of ints as a struct, this is a work around for RobotC's inability to pass an array to
00134  * a function.
00135  */
00136 typedef int tIntArray[MAX_ARR_SIZE];
00137 
00138 void clearI2CError(tSensors link, ubyte address);
00139 void clearI2Cbus(tSensors link);
00140 bool waitForI2CBus(tSensors link);
00141 bool writeI2C(tSensors link, tByteArray &request, tByteArray &reply, int replylen);
00142 bool writeI2C(tSensors link, tByteArray &request);
00143 
00144 
00145 /**
00146  * Clear out the error state on I2C bus by sending a bunch of dummy
00147  * packets.
00148  * @param link the port number
00149  * @param address the I2C address we're sending to
00150  */
00151 void clearI2CError(tSensors link, ubyte address) {
00152   ubyte error_array[2];
00153   error_array[0] = 1;           // Message size
00154   error_array[1] = address; // I2C Address
00155 
00156 #ifdef __COMMON_H_DEBUG__
00157   eraseDisplay();
00158   nxtDisplayTextLine(3, "rxmit: %d", ubyteToInt(error_array[1]));
00159   wait1Msec(2000);
00160 #endif // __COMMON_H_DEBUG__
00161 
00162   for (int i = 0; i < 5; i++) {
00163     sendI2CMsg(link, &error_array[0], 0);
00164     wait1Msec(5);
00165   }
00166 }
00167 
00168 
00169 /**
00170  * Wait for the I2C bus to be ready for the next message
00171  * @param link the port number
00172  * @return true if no error occured, false if it did
00173  */
00174 bool waitForI2CBus(tSensors link)
00175 {
00176   //TI2CStatus i2cstatus;
00177   while (true)
00178   {
00179     //i2cstatus = nI2CStatus[link];
00180     switch (nI2CStatus[link])
00181     //switch(i2cstatus)
00182     {
00183             case NO_ERR:
00184               return true;
00185 
00186             case STAT_COMM_PENDING:
00187               break;
00188 
00189             case ERR_COMM_CHAN_NOT_READY:
00190               break;
00191 
00192             case ERR_COMM_BUS_ERR:
00193         #ifdef __COMMON_H_DEBUG__
00194               PlaySound(soundLowBuzz);
00195               while (bSoundActive) {}
00196         #endif // __COMMON_H_DEBUG__
00197         return false;
00198     }
00199     EndTimeSlice();
00200   }
00201 }
00202 
00203 
00204 /**
00205  * Write to the I2C bus. This function will clear the bus and wait for it be ready
00206  * before any bytes are sent.
00207  * @param link the port number
00208  * @param request the data to be sent
00209  * @return true if no error occured, false if it did
00210  */
00211 bool writeI2C(tSensors link, tByteArray &request) {
00212 
00213 #if (__COMMON_H_SENSOR_CHECK__ == 1)
00214   //TSensorTypes type = SensorType[link];
00215 
00216   switch (SensorType[link])
00217   {
00218     case sensorI2CCustom:                 break;
00219     case sensorI2CCustom9V:               break;
00220     case sensorI2CCustomFast:             break;
00221     case sensorI2CCustomFast9V:           break;
00222     case sensorI2CCustomFastSkipStates9V: break;
00223     case sensorI2CCustomFastSkipStates:   break;
00224     default:
00225             hogCPU();
00226             PlaySound(soundException);
00227             eraseDisplay();
00228             nxtDisplayCenteredTextLine(0, "3rd Party Driver");
00229             nxtDisplayCenteredTextLine(1, "ERROR");
00230             nxtDisplayCenteredTextLine(2, "You have not");
00231             nxtDisplayCenteredTextLine(3, "setup the sensor");
00232             nxtDisplayCenteredTextLine(4, "port correctly. ");
00233             nxtDisplayCenteredTextLine(5, "Please refer to");
00234             nxtDisplayCenteredTextLine(6, "one of the");
00235             nxtDisplayCenteredTextLine(7, "examples.");
00236             wait1Msec(10000);
00237             StopAllTasks();
00238   }
00239 #endif
00240 
00241   if (!waitForI2CBus(link)) {
00242     clearI2CError(link, request[1]);
00243 
00244     // Let's try the bus again, see if the above packets flushed it out
00245     // clearI2CBus(link);
00246     if (!waitForI2CBus(link))
00247       return false;
00248   }
00249 
00250   sendI2CMsg(link, &request[0], 0);
00251 
00252   if (!waitForI2CBus(link)) {
00253     clearI2CError(link, request[1]);
00254     sendI2CMsg(link, &request[0], 0);
00255     if (!waitForI2CBus(link))
00256       return false;
00257   }
00258   return true;
00259 }
00260 
00261 
00262 /**
00263  * Write to the I2C bus. This function will clear the bus and wait for it be ready
00264  * before any bytes are sent.
00265  * @param link the port number
00266  * @param request the data to be sent
00267  * @param reply array to hold received data
00268  * @param replylen the number of bytes (if any) expected in reply to this command
00269  * @return true if no error occured, false if it did
00270  */
00271 bool writeI2C(tSensors link, tByteArray &request, tByteArray &reply, int replylen) {
00272   // clear the input data buffer
00273 
00274 #if (__COMMON_H_SENSOR_CHECK__ == 1)
00275   //TSensorTypes type = SensorType[link];
00276 
00277   switch (SensorType[link])
00278   {
00279     case sensorI2CCustom:                 break;
00280     case sensorI2CCustom9V:               break;
00281     case sensorI2CCustomFast:             break;
00282     case sensorI2CCustomFast9V:           break;
00283     case sensorI2CCustomFastSkipStates9V: break;
00284     case sensorI2CCustomFastSkipStates:   break;
00285     default:
00286             hogCPU();
00287             PlaySound(soundException);
00288             eraseDisplay();
00289             nxtDisplayCenteredTextLine(0, "3rd Party Driver");
00290             nxtDisplayCenteredTextLine(1, "ERROR");
00291             nxtDisplayCenteredTextLine(2, "You have not");
00292             nxtDisplayCenteredTextLine(3, "setup the sensor");
00293             nxtDisplayCenteredTextLine(4, "port correctly. ");
00294             nxtDisplayCenteredTextLine(5, "Please refer to");
00295             nxtDisplayCenteredTextLine(6, "one of the");
00296             nxtDisplayCenteredTextLine(7, "examples.");
00297             wait1Msec(10000);
00298             StopAllTasks();
00299   }
00300 #endif
00301 
00302   if (!waitForI2CBus(link)) {
00303     clearI2CError(link, request[1]);
00304 
00305     // Let's try the bus again, see if the above packets flushed it out
00306     // clearI2CBus(link);
00307     if (!waitForI2CBus(link))
00308       return false;
00309   }
00310 
00311   sendI2CMsg(link, &request[0], replylen);
00312 
00313   if (!waitForI2CBus(link)) {
00314     clearI2CError(link, request[1]);
00315     sendI2CMsg(link, &request[0], replylen);
00316     if (!waitForI2CBus(link))
00317       return false;
00318   }
00319 
00320   // ask for the input to put into the data array
00321   readI2CReply(link, &reply[0], replylen);
00322 
00323   return true;
00324 }
00325 
00326 /*
00327 bool I2CreadInt(tSensors link, ubyte address, ubyte reg, int &retval, tByteArray request, tByteArray reply, bool msbfirst = true)
00328 {
00329   request[0] = 2;            // Message size
00330   request[1] = address; // I2C Address
00331   request[2] = reg;
00332 
00333   if (!writeI2C(link, request, reply, 2))
00334     return false;
00335 
00336   retval = (msbfirst) ? reply[1] + (reply[0] << 8) : reply[0] + (reply[1] << 8);
00337 
00338   return true;
00339 }
00340 
00341 
00342 bool I2CreadLong(tSensors link, ubyte address, ubyte reg, int &retval, tByteArray request, tByteArray reply, bool msbfirst = true)
00343 {
00344   request[0] = 2;            // Message size
00345   request[1] = address; // I2C Address
00346   request[2] = reg;
00347 
00348   if (!writeI2C(link, request, reply, 4))
00349     return false;
00350 
00351   if (msbfirst)
00352     retval = (reply[0] << 24) + (reply[1] << 16) + (reply[2] <<  8) + reply[3];
00353   else
00354     retval = (reply[3] << 24) + (reply[2] << 16) + (reply[1] <<  8) + reply[0];
00355 
00356   return true;
00357 }
00358 
00359 
00360 bool I2CreadSByte(tSensors link, ubyte address, ubyte reg, sbyte &retval, tByteArray request, tByteArray reply)
00361 {
00362   request[0] = 2;            // Message size
00363   request[1] = address; // I2C Address
00364   request[2] = reg;
00365 
00366   if (!writeI2C(link, request, reply, 1))
00367     return false;
00368 
00369   retval =  (reply[0] >= 128) ? (int)reply[0] - 256 : (int)reply[0];
00370 
00371   return true;
00372 }
00373 
00374 
00375 bool I2CreadUByte(tSensors link, ubyte address, ubyte reg, ubyte &retval, tByteArray request, tByteArray reply)
00376 {
00377   request[0] = 2;            // Message size
00378   request[1] = address; // I2C Address
00379   request[2] = reg;
00380 
00381   if (!writeI2C(link, request, reply, 1))
00382     return false;
00383 
00384   retval =  reply[0];
00385 
00386   return true;
00387 }
00388 */
00389 
00390 /**
00391  * Create a unique ID (UID) for an NXT.  This based on the last 3 bytes
00392  * of the Bluetooth address.  The first 3 bytes are manufacturer
00393  * specific and identical for all NXTs and are therefore not used.
00394  * @return a unique ID for the NXT.
00395  */
00396 long getUID() {
00397   TBTAddress btAddr;
00398   getBTAddress(btAddr);
00399 
00400   // Only last 3 bytes are unique in the BT address, the other three are for the
00401   // manufacturer (LEGO):  http://www.coffer.com/mac_find/?string=lego
00402    return (long)btAddr[5] + ((long)btAddr[4] << 8) + ((long)btAddr[3] << 16);
00403 }
00404 
00405 
00406 #define STRTOK_MAX_TOKEN_SIZE 20
00407 #define STRTOK_MAX_BUFFER_SIZE 50
00408 
00409 /**
00410  * Tokenise an array of chars, using a seperator
00411  * @param buffer pointer to buffer we're parsing
00412  * @param token pointer to buffer to hold the tokens as we find them
00413  * @param seperator the seperator used between tokens
00414  * @return true if there are still tokens left, false if we're done
00415  */
00416 bool strtok(char *buffer, char *token, char *seperator)
00417 {
00418   int pos = StringFind(buffer, seperator);
00419   char t_buff[STRTOK_MAX_BUFFER_SIZE];
00420 
00421   // Make sure we zero out the buffer and token
00422   memset(token, 0, STRTOK_MAX_TOKEN_SIZE);
00423   memset(&t_buff[0], 0, STRTOK_MAX_BUFFER_SIZE);
00424 
00425   // Looks like we found a seperator
00426   if (pos >= 0)
00427   {
00428     // Copy the first token into the token buffer, only if the token is
00429     // not an empty one
00430     if (pos > 0)
00431       memcpy(token, buffer, pos);
00432     // Now copy characters -after- the seperator into the temp buffer
00433     memcpy(&t_buff[0], buffer + (pos + 1), strlen(buffer) - pos);
00434     // Zero out the real buffer
00435     memset(buffer, 0, strlen(buffer) + 1);
00436     // Copy the temp buffer, which now only contains everything after the previous
00437     // token into the buffer for the next round.
00438     memcpy(buffer, &t_buff[0], strlen(&t_buff[0]));
00439     return true;
00440   }
00441   // We found no seperator but the buffer still contains a string
00442   // This can happen when there is no trailing seperator
00443   else if(strlen(buffer) > 0)
00444   {
00445     // Copy the token into the token buffer
00446     memcpy(token, buffer, strlen(buffer));
00447     // Zero out the remainder of the buffer
00448     memset(buffer, 0, strlen(buffer) + 1);
00449     return true;
00450   }
00451   return false;
00452 }
00453 
00454 
00455 #endif // __COMMON_H__
00456 
00457 /*
00458  * $Id: common.h 134 2013-03-12 18:15:17Z xander $
00459  */
00460 /* @} */
00461 /* @} */