Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
691 lines (607 sloc) 19.6 KB
/*!@addtogroup common_includes
* @{
* @defgroup common Main common include
* Commonly used functions used by drivers
* @{
*/
/** \file common.h
* \brief Commonly used functions used by drivers.
*
* common.h provides a number of frequently used functions that are useful for writing
* drivers.
* License: You may use this code as you wish, provided you give credit where its due.
*
* THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 4.10 AND HIGHER
*
* Changelog:
* - 0.1: Initial release
* - 0.2: Added version check to issue error when compiling with RobotC < 1.46
* - 0.2: Added __COMMON_H_DEBUG__ to enable/disable sounds when an I2C error occurs
* - 0.2: Removed bool waitForI2CBus(tSensors link, bool silent)
* - 0.3: clearI2CError() added to make writeI2C more robust, I2C bus errors are now handled
* better.
* - 0.4: Added HiTechnic SMUX functions
* - 0.5: Added clip function (Tom Roach)
* - 0.6: clearI2CBus is now conditionally compiled into the FW. Only RobotC < 1.57 needs it.
* - 0.7: ubyteToInt(byte byteVal) modified, works better with 1.57+
* - 0.8: ubyte used for arrays for firmware version 770 and higher<br>
* added support for new colour sensor<br>
* added better handling for when sensor is not configured properly
* - 0.9: added bool HTSMUXsetMode(tSensors link, byte channel, byte mode) prototype<br>
* added short HTSMUXreadAnalogue(tMUXSensor muxsensor)<br>
* added HTSMUXSensorType HTSMUXreadSensorType(tMUXSensor muxsensor)<br>
* added bool HTSMUXreadPort(tMUXSensor muxsensor, tByteArray &result, short numbytes, short offset)<br>
* added bool HTSMUXreadPort(tMUXSensor muxsensor, tByteArray &result, short numbytes)<br>
* added bool HTSMUXsetMode(tMUXSensor muxsensor, byte mode)<br>
* added bool HTSMUXsetAnalogueActive(tMUXSensor muxsensor)<br>
* added bool HTSMUXsetAnalogueInactive(tMUXSensor muxsensor)<br>
* corrected function description for HTSMUXSensorType()
* - 0.10: Removed unnecessary read from HTSMUXsendCommand()
* - 0.11: Added long uByteToLong(byte a1, byte a2, byte a3, byte a4);
* - 0.12: Added HTSMUXreadPowerStatus(tSensors link)<br>
* Added short round(float fl)
* - 0.13: Added motor mux types and data structs
* - 0.14: Added check for digital sensors to prevent conflict with built-in drivers\n
* Changed clearI2CError to take ubyte for address, thanks Aswin
* - 0.15: Removed motor mux and sensor mux functions and types out
* - 0.16: Added max() and min() functions by Mike Henning, Max Bareiss
*
* \author Xander Soldaat (xander_at_botbench.com)
* \date 27 April 2011
* \version 0.16
*/
#pragma systemFile
#ifndef __COMMON_H__
#define __COMMON_H__
// #define DISABLE_ERROR_REPORTING
#ifndef DISABLE_ERROR_REPORTING
#pragma debuggerWindows("debugStream");
#endif // DISABLE_ERROR_REPORTING
//#undef __COMMON_H_DEBUG__
//#define __COMMON_H_DEBUG__
/*!< define this as 0 to remove the check */
#ifndef __COMMON_H_SENSOR_CHECK__
#define __COMMON_H_SENSOR_CHECK__ 1
#else
#warn "sensor checking disabled, I hope you know what you are doing!"
#endif
#include "firmwareVersion.h"
#if (kRobotCVersionNumeric < 410)
#error "These drivers are only supported on RobotC version 4.10 or higher"
#endif
#ifndef MAX_ARR_SIZE
/**
* Maximum buffer size for byte_array, can be overridden in your own program.
* It's 17 bytes big because the max I2C buffer size is 16, plus 1 byte to denote
* packet length.
*/
#define MAX_ARR_SIZE 17
#endif
/**
* This define returns the smaller of the two numbers
*/
#define min2(a, b) (a < b ? a : b)
/**
* This define returns the smallest of the three numbers
*/
#define min3(a, b, c) (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c)
/**
* This function returns the bigger of the two numbers
*/
#define max2(a, b) (a > b ? a : b)
/**
* This function returns the biggest of the three numbers
*/
#define max3(a, b, c) (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c)
/**
* Returns x if it is between min and max. If outside the range,
* it returns min or max.
*/
#define clip(a, b, c) min2(c, max2(b, a))
#ifndef SPORT
#define SPORT(X) (X / 4) /*!< Convert tMUXSensor to sensor port number */
#endif
#ifndef MPORT
#define MPORT(X) (X % 4) /*!< Convert tMUXSensor to MUX port number */
#endif
/*!< Sensor and SMUX port combinations */
typedef enum tMUXSensor {
msensor_S1_1 = 0,
msensor_S1_2 = 1,
msensor_S1_3 = 2,
msensor_S1_4 = 3,
msensor_S2_1 = 4,
msensor_S2_2 = 5,
msensor_S2_3 = 6,
msensor_S2_4 = 7,
msensor_S3_1 = 8,
msensor_S3_2 = 9,
msensor_S3_3 = 10,
msensor_S3_4 = 11,
msensor_S4_1 = 12,
msensor_S4_2 = 13,
msensor_S4_3 = 14,
msensor_S4_4 = 15
} tMUXSensor;
typedef struct
{
ubyte request[17];
ubyte requestLen;
ubyte reply[17];
ubyte replyLen;
ubyte address;
tSensors port;
TSensorTypes type;
} tI2CData, *tI2CDataPtr;
/**
* Array of bytes as a struct, this is a work around for RobotC's inability to pass an array to
* a function.
*/
typedef ubyte tByteArray[MAX_ARR_SIZE];
typedef sbyte tsByteArray[MAX_ARR_SIZE];
typedef ubyte tMassiveArray[128]; /*!< 128 byte array for very large blobs of data */
typedef ubyte tHugeByteArray[64]; /*!< 64 byte array for very large blobs of data */
typedef ubyte tBigByteArray[32]; /*!< 32 byte array for large blobs of data */
typedef ubyte tIPaddr[4]; /*!< Struct for holding an IP address */
/**
* Array of ints as a struct, this is a work around for RobotC's inability to pass an array to
* a function.
*/
typedef short tIntArray[MAX_ARR_SIZE];
void clearI2CError(tSensors link, ubyte address);
void clearI2Cbus(tSensors link);
bool waitForI2CBus(tSensors link);
bool writeI2C(tSensors link, tByteArray &request, tByteArray &reply, short replylen);
bool writeI2C(tSensors link, tByteArray &request);
/**
* Clear out the error state on I2C bus by sending a bunch of dummy
* packets.
* @param link the port number
* @param address the I2C address we're sending to
*/
#ifdef NXT
void clearI2CError(tI2CDataPtr data) {
ubyte error_array[2];
error_array[0] = 1; // Message size
error_array[1] = data->address; // I2C Address
#ifdef __COMMON_H_DEBUG__
eraseDisplay();
displayTextLine(3, "rxmit: %d", error_array[1]);
sleep(2000);
#endif // __COMMON_H_DEBUG__
for (short i = 0; i < 5; i++) {
sendI2CMsg(data->port, &error_array[0], 0);
sleep(10);
}
}
#endif
/**
* Clear out the error state on I2C bus by sending a bunch of dummy
* packets.
* @param link the port number
* @param address the I2C address we're sending to
*/
//#ifdef NXT
void clearI2CError(tSensors link, ubyte address) {
ubyte error_array[2];
error_array[0] = 1; // Message size
error_array[1] = address; // I2C Address
#ifdef __COMMON_H_DEBUG__
eraseDisplay();
displayTextLine(3, "rxmit: %d", error_array[1]);
sleep(2000);
#endif // __COMMON_H_DEBUG__
for (short i = 0; i < 5; i++) {
sendI2CMsg(link, &error_array[0], 0);
waitForI2CBus(link);
}
}
//#endif
/**
* Wait for the I2C bus to be ready for the next message
* @param link the port number
* @return true if no error occured, false if it did
*/
bool waitForI2CBus(tSensors link)
{
while (true)
{
TI2CStatus i2cstatus = nI2CStatus[link];
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("nI2CStatus[%d]: %d", link, i2cstatus);
#endif // DEBUG_COMMON_H
switch(i2cstatus)
{
#if defined(NXT)
case NO_ERR:
return true;
case STAT_COMM_PENDING:
break;
case ERR_COMM_CHAN_NOT_READY:
break;
case ERR_COMM_BUS_ERR:
#else // this must be an EV3
case i2cStatusStopped:
case i2cStatusNoError:
return true;
case i2cStatusPending:
case i2cStatusStartTransfer:
break;
case i2cStatusFailed:
case i2cStatusBadConfig:
#endif
#ifdef __COMMON_H_DEBUG__
playSound(soundLowBuzz);
while (bSoundActive) {}
#endif // __COMMON_H_DEBUG__
return false;
}
sleep(1);
}
}
/**
* Wait for the I2C bus to be ready for the next message
* @param link the port number
* @return true if no error occured, false if it did
*/
bool waitForI2CBus(tI2CDataPtr data)
{
while (true)
{
//i2cstatus = nI2CStatus[link];
switch (nI2CStatus[data->port])
//switch(i2cstatus)
{
#ifdef NXT
case NO_ERR:
return true;
case STAT_COMM_PENDING:
break;
case ERR_COMM_CHAN_NOT_READY:
break;
case ERR_COMM_BUS_ERR:
#else // this must be an EV3
case i2cStatusStopped:
case i2cStatusNoError:
return true;
case i2cStatusPending:
case i2cStatusStartTransfer:
break;
case i2cStatusFailed:
case i2cStatusBadConfig:
#endif
#ifdef __COMMON_H_DEBUG__
playSound(soundLowBuzz);
while (bSoundActive) {}
#endif // __COMMON_H_DEBUG__
return false;
}
sleep(1);
}
}
bool writeI2C(tI2CDataPtr data) {
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("writeI2C(tI2CDataPtr data) called"); sleep(200);
#endif // DEBUG_COMMON_H
#if (__COMMON_H_SENSOR_CHECK__ == 1)
//TSensorTypes type = SensorType[link];
switch (SensorType[data->port])
{
case sensorSONAR: break;
case sensorI2CCustom: break;
case sensorI2CCustom9V: break;
#ifdef EV3
case sensorEV3_EnergyMeter: break; // for some reason, some I2C sensors are identified as this
case sensorEV3_GenericI2C: break;
#else // This is an NXT
case sensorI2CCustomFast: break;
case sensorI2CCustomFast9V: break;
case sensorI2CCustomFastSkipStates9V: break;
case sensorI2CCustomFastSkipStates: break;
#endif // EV3/NXT
default:
hogCPU();
playSound(soundException);
eraseDisplay();
#ifdef EV3
setLEDColor(ledRedPulse);
#endif // EV3
writeDebugStreamLine("ERROR, You have not setup the sensor port correctly. ");
writeDebugStreamLine("Please refer to one of the examples.");
writeDebugStreamLine("Detected SensorType on port[%d]: %d", data->port, SensorType[data->port]);
sleep(10000);
stopAllTasks();
}
#endif // __COMMON_H_SENSOR_CHECK__
#ifdef NXT
if (!waitForI2CBus(data->port)) {
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("waiting for the bus");
#endif // DEBUG_COMMON_H
clearI2CError(data->port, data->address);
// Let's try the bus again, see if the above packets flushed it out
// clearI2CBus(link);
if (!waitForI2CBus(data->port))
return false;
}
#endif
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("writeI2C: port: %d, addr: 0x%02X, len: %d", data->port, data->address, data->requestLen); sleep(200);
#endif
#ifdef DEBUG_COMMON_H
writeDebugStream("writeI2C: data->request: ");
for (int i = 0; i < (data->requestLen + 1); i++)
{
writeDebugStream("0x%02X ", data->request[i]);
}
writeDebugStream("\n");
#endif // DEBUG_COMMON_H
sendI2CMsg(data->port, &data->request[0], data->replyLen);
if (!waitForI2CBus(data)) {
#ifdef EV3
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("waiting for the bus has failed"); sleep(200);
#endif
return false;
#else
clearI2CError(data->port, data->address);
sendI2CMsg(data->port, &data->request[0], data->replyLen);
if (!waitForI2CBus(data))
return false;
#endif
}
if (data->replyLen == 0)
return true;
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("writeI2C: initiating read: data->replyLen: %d", data->replyLen); sleep(200);
#endif // DEBUG_COMMON_H
readI2CReply(data->port, &data->reply[0], data->replyLen);
#ifdef EV3
return waitForI2CBus(data);
#else
return true;
#endif // EV3
}
/**
* Write to the I2C bus. This function will clear the bus and wait for it be ready
* before any bytes are sent.
* @param link the port number
* @param request the data to be sent
* @return true if no error occured, false if it did
*/
bool writeI2C(tSensors link, tByteArray &request) {
#if (__COMMON_H_SENSOR_CHECK__ == 1)
//TSensorTypes type = SensorType[link];
switch (SensorType[link])
{
case sensorSONAR: break;
case sensorI2CCustom: break;
case sensorI2CCustom9V: break;
#ifdef EV3
case sensorEV3_EnergyMeter: break; // for some reason, some I2C sensors are identified as this
case sensorEV3_GenericI2C: break;
#else // This is an NXT
case sensorI2CCustomFast: break;
case sensorI2CCustomFast9V: break;
case sensorI2CCustomFastSkipStates9V: break;
case sensorI2CCustomFastSkipStates: break;
#endif // EV3/NXT
default:
hogCPU();
playSound(soundException);
eraseDisplay();
#ifdef EV3
setLEDColor(ledRedPulse);
#endif // EV3
writeDebugStreamLine("ERROR, You have not setup the sensor port correctly. ");
writeDebugStreamLine("Please refer to one of the examples.");
writeDebugStreamLine("Detected SensorType on port[%d]: %d", link, SensorType[link]);
sleep(10000);
stopAllTasks();
}
#endif
// This is not required for the EV3
#ifdef NXT
if (!waitForI2CBus(link)) {
clearI2CError(link, request[1]);
// Let's try the bus again, see if the above packets flushed it out
// clearI2CBus(link);
if (!waitForI2CBus(link))
return false;
}
#endif // __COMMON_H_SENSOR_CHECK__
sendI2CMsg(link, &request[0], 0);
if (!waitForI2CBus(link)) {
#ifdef EV3
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("waiting for the bus has failed"); sleep(200);
#endif
return false;
#else
clearI2CError(link, request[1]);
sendI2CMsg(link, &request[0], 0);
if (!waitForI2CBus(link))
return false;
#endif
}
return true;
}
/**
* Write to the I2C bus. This function will clear the bus and wait for it be ready
* before any bytes are sent.
* @param link the port number
* @param request the data to be sent
* @param reply array to hold received data
* @param replylen the number of bytes (if any) expected in reply to this command
* @return true if no error occured, false if it did
*/
bool writeI2C(tSensors link, tByteArray &request, tByteArray &reply, short replylen) {
// clear the input data buffer
#if (__COMMON_H_SENSOR_CHECK__ == 1)
//TSensorTypes type = SensorType[link];
switch (SensorType[link])
{
case sensorSONAR: break;
case sensorI2CCustom: break;
case sensorI2CCustom9V: break;
#ifdef EV3
case sensorEV3_EnergyMeter: break; // for some reason, some I2C sensors are identified as this
case sensorEV3_GenericI2C: break;
#else // This is an NXT
case sensorI2CCustomFast: break;
case sensorI2CCustomFast9V: break;
case sensorI2CCustomFastSkipStates9V: break;
case sensorI2CCustomFastSkipStates: break;
#endif // EV3/NXT
default:
hogCPU();
playSound(soundException);
eraseDisplay();
#ifdef EV3
setLEDColor(ledRedPulse);
#endif // EV3
writeDebugStreamLine("ERROR, You have not setup the sensor port correctly. ");
writeDebugStreamLine("Please refer to one of the examples.");
writeDebugStreamLine("Detected SensorType on port[%d]: %d", link, SensorType[link]);
sleep(10000);
stopAllTasks();
}
#endif // __COMMON_H_SENSOR_CHECK__
// This is not required for the EV3
#ifdef NXT
if (!waitForI2CBus(link)) {
clearI2CError(link, request[1]);
// Let's try the bus again, see if the above packets flushed it out
// clearI2CBus(link);
if (!waitForI2CBus(link))
return false;
}
#endif
sendI2CMsg(link, &request[0], replylen);
if (!waitForI2CBus(link)) {
#ifdef EV3
#ifdef DEBUG_COMMON_H
writeDebugStreamLine("waiting for the bus has failed"); sleep(200);
#endif
return false;
#else
clearI2CError(link, request[1]);
sendI2CMsg(link, &request[0], replylen);
if (!waitForI2CBus(link))
return false;
#endif
}
// ask for the input to put into the data array
readI2CReply(link, &reply[0], replylen);
#ifdef EV3
return waitForI2CBus(link);
#else
return true;
#endif // EV3
}
/**
* Create a unique ID (UID) for an NXT. This based on the last 3 bytes
* of the Bluetooth address. The first 3 bytes are manufacturer
* specific and identical for all NXTs and are therefore not used.
* @return a unique ID for the NXT.
*/
long getUID() {
#ifdef NXT
TBTAddress btAddr;
getBTAddress(btAddr);
// Only last 3 bytes are unique in the BT address, the other three are for the
// manufacturer (LEGO): http://www.coffer.com/mac_find/?string=lego
return (long)btAddr[5] + ((long)btAddr[4] << 8) + ((long)btAddr[3] << 16);
#else
return 0;
#endif // NXT
}
#define STRTOK_MAX_TOKEN_SIZE 20
#define STRTOK_MAX_BUFFER_SIZE 50
/**
* Tokenise an array of chars, using a seperator
* @param buffer pointer to buffer we're parsing
* @param token pointer to buffer to hold the tokens as we find them
* @param seperator the seperator used between tokens
* @return true if there are still tokens left, false if we're done
*/
bool strtok(char *buffer, char *token, char *seperator)
{
short pos = stringFind(buffer, seperator);
char t_buff[STRTOK_MAX_BUFFER_SIZE];
// Make sure we zero out the buffer and token
memset(token, 0, STRTOK_MAX_TOKEN_SIZE);
memset(&t_buff[0], 0, STRTOK_MAX_BUFFER_SIZE);
// Looks like we found a seperator
if (pos >= 0)
{
// Copy the first token into the token buffer, only if the token is
// not an empty one
if (pos > 0)
memcpy(token, buffer, pos);
// Now copy characters -after- the seperator into the temp buffer
memcpy(&t_buff[0], buffer + (pos + 1), strlen(buffer) - pos);
// Zero out the real buffer
memset(buffer, 0, strlen(buffer) + 1);
// Copy the temp buffer, which now only contains everything after the previous
// token into the buffer for the next round.
memcpy(buffer, &t_buff[0], strlen(&t_buff[0]));
return true;
}
// We found no seperator but the buffer still contains a string
// This can happen when there is no trailing seperator
else if(strlen(buffer) > 0)
{
// Copy the token into the token buffer
memcpy(token, buffer, strlen(buffer));
// Zero out the remainder of the buffer
memset(buffer, 0, strlen(buffer) + 1);
return true;
}
return false;
}
typedef enum tXButton
{
#if defined(EV3)
xButtonLeft = buttonLeft,
xButtonRight = buttonRight,
xButtonBack = buttonBack,
xButtonEnter = buttonEnter,
xButtonAny = buttonAny
#elif defined(NXT)
xButtonLeft = kLeftButton,
xButtonRight = kRightButton,
xButtonBack = kExitButton,
xButtonEnter = kEnterButton,
xButtonAny = 100
#endif
} tXButton;
bool getXbuttonValue(tXButton button)
{
tXButton currButton;
#if defined(EV3)
return getButtonPress((TEV3Buttons)button);
#elif defined(NXT)
currButton = (tXButton)nNxtButtonPressed;
if ((button == xButtonAny) && (currButton != kNoButton))
return true;
else
return (currButton == button) ? true : false;
#endif
}
void resetSensorConn(tSensors link)
{
#if defined (EV3)
setSensorAutoID(link, false);
sleep(10);
setSensorConnectionType(link, CONN_NONE);
sleep(10);
setSensorAutoID(link, true);
sleep(1000);
#else
return;
#endif
}
#endif // __COMMON_H__
/* @} */
/* @} */
You can’t perform that action at this time.