/* * * * * * * * * * * * * * * * * * * * * * * @file MyroC-sensor.c * @brief Implementation file for the MyroC sensor commands * * Revision History * * Version 1.0 based on a C++ package by April O'Neill, David Cowden, * Dilan Ustek, Erik Opavsky, and Henry M. Walker * * Developers of the C package for Linux: * Creators Version 2.0 (C functions for utilities,general,sensors,movement): * Spencer Liberto * Dilan Ustek * Jordan Yuan * Henry M. Walker * Contributors Version 2.2-2.3: (C functions for image processing) * Anita DeWitt * Jason Liu * Nick Knoebber * Vasilisa Bashlovkina * Revision for Version 2.4: (image row/column made to match matrix notation) * Henry M. Walker * * Revisions for Version 3.0 * Henry M. Walker * * C ported to Macintosh * Linux/Mac differences required for connections — otherwise same code * OpenGL used to display images, replacing ImageMagick * same [new] code used for both Linux and Macintosh * 1 process for robot control * 1 process needed for each titled window (not each image, as in 2.2-2.4) * Blocking options (negative duration parameter) * utilize separate thread timer * MyroC implementation files organized by user function as follows: * * Revisions for Version 3.1 * Henry M. Walker * * Picture struct and image functions revised to allow * 192 by 256 images from origial Fluke camera * 266 x 427 low-resolution images from Fluke 2 * (high-resolution (800 x 1280) too large for more than 2-4 * variables on run-time stack) * storage , retrieval, and display of any images up to 266 x 427 * * This program and all MyroC software is licensed under the Creative Commons * Attribution-Noncommercial-Share Alike 3.0 United States License. * Details available at http://creativecommons.org/licenses/by-nc-sa/3.0/us/ * ********************** implementation overview ************************* > indicates functions implemented in this file 0. LOW-LEVEL UTILITY (defined in MyroC-utilities.h implemented in * MyroC-connect.c) rSend rReceive rSetOpeningExchange rSetBluetoothEcho rSetEchoMode rCheckHardwareVersionSetCameraSize 1. GENERAL (mostly MyroC-general.c) 2. SENSOR (MyroC-sensors.c) rConnect (* MyroC-connect.c) a.Scribbler Sensors rDisconnect (* MyroC-connect.c) > rGetLightsAll rSetConnection > rGetLightTxt rFinishProcessing > rGetIRAll rSetVolume > rGetIRTxt rBeep > rGetLine rBeep2 rSetName b.Fluke Sensors rGetName > rGetObstacleAll rSetForwardness > rGetObstacleTxt rGetForwardness > rGetBrightAll rSetLEDFront > rGetBrightTxt rSetLEDBack > rSetIRPower rGetBattery rGetStall Note: * MyroC-connect requires special knowledge of a Mac or Linux environment, yielding two versions: MyroC-connect-mac.c MyroC-connect-linux.c 3. MOVEMENT (MyroC-movement.c) 4. PICTURES (files as indicated) rTurnLeft rGetCameraSize (MyroC-camera.c) rTurnRight rTakePicture (MyroC-camera.c) rTurnSpeed rDisplayPicture (MyroC-display.c) rForward rWaitTimedImageDisplay (MyroC-display.c) rFastForward rSavePicture (MyroC-image-file.c) rBackward rLoadPicture (MyroC-image-file.c) rMotors rStop rHardStop ************************************************************************ */ #include "MyroC.h" #include "MyroC-utilities.h" #include #include #include #include /**************************************************************************/ /* 2. SENSORS - SENSORS - SENSORS - SENSORS - SENSORS - SENSORS - SENSORS */ /**************************************************************************/ /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ /* Group a: Scribbler Sensors */ /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ /** * @brief Get the average values of each of the three light sensors in an array. * Values of each light sensor can somewhat (typically under 5%-10%). * To even out variability, the sensor can be queried sampleSize times * and an average obtained. * @param lightSensors array to store intensity values * @param sampleSize how many readings are taken for each sensor * @pre space already allocated for lightSensors array * sampleSize > 0 * @post * lightSensors[0] gives average value for left sensor * lightSensors[1] gives average value for middle sensor * lightSensors[2] gives average value for right sensor * Intensity values near 0 represent bright light * Intensities may extend to about 65000 for a very dark region. */ void rGetLightsAll (int lightSensors[3], int sampleSize) { char message[9]; unsigned char receivedMessage[6]; message[0] = 70; message[1] = 0; message[2] = 0; message[3] = 0; message[4] = 0; message[5] = 0; message[6] = 0; message[7] = 0; message[8] = 0; lightSensors[0] = 0; lightSensors[1] = 0; lightSensors[2] = 0; int i; for(i = 0; i < sampleSize; i++){ rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetLightsAll"); rReceive(receivedMessage, 6); /* sensor values returned as high-byte/low-byte pairs combine bytes with bitwise-or */ lightSensors[0] += (int)receivedMessage[0] << 8 | (int) receivedMessage[1]; lightSensors[1] += (int)receivedMessage[2] << 8 | (int) receivedMessage[3]; lightSensors[2] += (int)receivedMessage[4] << 8 | (int) receivedMessage[5]; } lightSensors[0] /= sampleSize; lightSensors[1] /= sampleSize; lightSensors[2] /= sampleSize; } /** * @brief Get the average values of a specified light sensor. * Values of each light sensor can vary somewhat (typically under 5%-10%). * To even out variability, the sensor can be queried sampleSize times * and an average obtained. * @param sensorName name of the light sensor * @pre sensorName is "left", "center", "middle", or "right" * (not case sensitive) * designations "center" and "middle" are alternatives * for the same light sensor * @param sampleSize how many readings are taken for the sensor * @pre sampleSize > 0 * @return reading from the specified light sensor, averaged * over sampleSize number of data samples * if sensorName invalid, returns -1.0 */ int rGetLightTxt (const char * sensorName, int sampleSize) { int len = strlen(sensorName) + 1; char workingString [len]; int i; for (i = 0; i < len; i++) workingString[i] = tolower (sensorName[i]); char message[9]; unsigned char receivedMessage[2]; int summedValue = 0; if(!strcmp(workingString, "left")){ message[0]= 67; } else if ((!strcmp(workingString, "center")) || (!strcmp(workingString, "middle"))){ message[0]= 68; } else if (!strcmp(workingString, "right")){ message[0]= 69; } else return -1.0; message[1] = 0; message[2] = 0; message[3] = 0; message[4] = 0; message[5] = 0; message[6] = 0; message[7] = 0; message[8] = 0; /* query sensor specified number of times and average */ for(i = 0; i < sampleSize; i++){ rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetLightTxt"); rReceive(receivedMessage, 2); /* sensor values returned as high-byte/low-byte pairs combine bytes with bitwise-or */ summedValue += (int)receivedMessage[0] << 8 | (int) receivedMessage[1]; } return summedValue / sampleSize; } /** * @brief Get an array of true/false values regarding the presence of an obstacle, * based on the average values of each of the three IR sensors. * Since readings of each light sensor can vary substantially, * each sensor can be queried sampleSize times and an average obtained. * @param irSensors array to store intensity values * @param sampleSize how many readings are taken for each sensor * @pre space already allocated for irSensors array * sampleSize > 0 * @post * irSensors[0] checks obstacle for left sensor * irSensors[1] checks obstacle for right sensor * @post for each irSensors array value * return 0 indicates no obstacle detected * return 1 indicates obstacle detected */ void rGetIRAll (int irSensors[2], int sampleSize) { char message[9]; unsigned char receivedMessage[2]; int summedValues[2]; message[0] = 73; message[1] = 0; // not used message[2] = 0; // not used message[3] = 0; // not used message[4] = 0; // not used message[5] = 0; // not used message[6] = 0; // not used message[7] = 0; // not used message[8] = 0; // not used summedValues[0] = 0; summedValues[1] = 0; int i; for(i = 0; i < sampleSize; i++){ rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetIRAll"); rReceive(receivedMessage, 2); summedValues[0] += receivedMessage[0]; summedValues[1] += receivedMessage[1]; } /* obstacle sensor is misnamed, returning 1 if no obstacle and 0 if obstacle the following code switches the output, so 1 means obstacle exists */ /* obstacle present if obstacle found in at least half the queries */ irSensors[0] = (summedValues[0] < (sampleSize/2)); irSensors[1] = (summedValues[1] < (sampleSize/2)); } /** * @brief Use specified IR sensor to determine if obstacle is present. * Since values of each light sensor can vary substantially, * the sensor can be queried sampleSize times and an average obtained. * @param sensorName name of the light sensor * @pre sensorName is "left" or "right" * (not case sensitive) * @param sampleSize how many readings are taken for the sensor * @pre sampleSize > 0 * @return true/false (0/1) determination of obstacle, based on IR * sensorName sensor, averaged over sampleSize number of * data samples * @post return 0 indicates no obstacle detected * return 1 indicates obstacle detected */ int rGetIRTxt (const char * sensorName, int sampleSize) { int len = strlen(sensorName) + 1; char workingString [len]; int i; for (i = 0; i < len; i++) workingString[i] = tolower (sensorName[i]); char message[9]; unsigned char receivedMessage[1]; int summedValue = 0;; if(strcmp(workingString, "left") == 0){ message[0]= 71; } else if (strcmp(workingString, "right") == 0){ message[0]= 72; } else { return -1; } message[1]= 0; message[2]= 0; message[3]= 0; message[4]= 0; message[5]= 0; message[6]= 0; message[7]= 0; message[8]= 0; /* query sensors needed number of times to get average */ for(i = 0; i < sampleSize; i++){ rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetIRTxt"); rReceive(receivedMessage, 1); summedValue += receivedMessage[0]; } /* obstacle sensor is misnamed, returning 1 if no obstacle and 0 if obstacle the following code switches the output, so 1 means obstacle exists */ /* obstacle present if obstacle found in at least half the queries */ return (summedValue <= (sampleSize/2)); } /** * @brief Use Scribbler 2 line sensors of Scribbler to check for a dark line * on a light surface under the robot. * Since values of each light sensor can vary substantially, * the sensor can be queried sampleSize times and an average obtained. * @warning results of these sensors may be flakey! * @param lineSensors array to store line values detected * @param sampleSize how many readings are taken for each sensor * @pre space already allocated for lineSensors array * sampleSize > 0 * @post lineSensors[0] checks left sensor for line * lineSensors[1] checks right sensor for line * (left/right based on Scribbler-forward orientation) * @post for each line sensors array value * return 0 indicates line is identified (light color under Scribbler) * return 1 indicates line is not identified (dark color) */ void rGetLine (int lineSensors[2], int sampleSize) { char message[9]; unsigned char receivedMessage[2]; int summedValues[2]; message[0] = 76; message[1] = 0; // not used message[2] = 0; // not used message[3] = 0; // not used message[4] = 0; // not used message[5] = 0; // not used message[6] = 0; // not used message[7] = 0; // not used message[8] = 0; // not used summedValues[0] = 0; summedValues[1] = 0; int i; for(i=0; i= (sampleSize/2)); lineSensors[1] = (summedValues[1] >= (sampleSize/2)); } /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ /* Group b: Fluke Sensors */ /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ /** * @brief Set the amount of power for the dongle's IR sensors * @param power the desired power level for the IR sensors * @pre power is between 0 and 255 (inclusive) */ void rSetIRPower (int power) { if (power > 255) power = 255; if (power < 0) power = 0; char message[2]; message[0] = 120; message[1] = power; rSend(message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetIRPower"); } /** * @brief Get the average values of the three obstacle sensors in an array. * Since readings of each obstacle sensor can vary substantially (successive * readings may differ by several hundred or more), * each sensor can be queried sampleSize times and an average obtained. * @param obstSensors array to store intensity values * @param sampleSize how many readings are taken for each sensor * @pre space already allocated for obstSensors array * sampleSize > 0 * @post obstSensors[0] gives average value for left sensor * obstSensors[1] gives average value for middle sensor * obstSensors[2] gives average value for right sensor * Returned values are between 0 and 6400 * Obstacle values near 0 represent no obstacle seen * Obstacle values may approach 6400 as obstacle gets close. * @warning As battery degrades, sensor readings degrade, * yielding systematically lower numbers. */ void rGetObstacleAll (int obstSensors[3], int sampleSize) { obstSensors[0] = rGetObstacleTxt("left", sampleSize); obstSensors[1] = rGetObstacleTxt("middle", sampleSize); obstSensors[2] = rGetObstacleTxt("right", sampleSize); } /** * @brief Get the average values of a specified obstacle (IR) sensor. * Since values of each obstacle sensor can vary substantially (successive * readings may differ by several hundred or more), * the sensor can be queried sampleSize times and an average obtained. * @param sensorName name of the obstacle sensor * @pre sensorName is "left", "center", "middle", or "right" * (not case sensitive) * designations "center" and "middle" are alternatives * for the same light sensor * @param sampleSize how many readings are taken for the sensor * @pre space already allocated for vals array * sampleSize > 0 * @return reading from the specified obstacle sensor, averaged * over sampleSize number of data samples * Returned values are between 0 and 6400 * Obstacle values near 0 represent no obstacle seen * Obstacle values may approach 6400 as obstacle gets close. * @warning As battery degrades, sensor values degrade, * yielding systematically lower numbers. */ int rGetObstacleTxt (const char * sensorName, int sampleSize) { int len = strlen(sensorName) + 1; char workingString [len]; int i; for (i = 0; i < len; i++) workingString[i] = tolower (sensorName[i]); char message[1]; unsigned char result[2]; int summedValue = 0; if (strcmp("left", workingString) == 0) { message[0] = 85; } else if ((strcmp("middle", workingString) == 0) || (strcmp("center", workingString) == 0)) { message[0] = 86; } else if (strcmp("right", workingString) == 0) { message[0] = 87; } else return -1; for(i = 0; i < sampleSize; i++){ rSend (message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rGetObstacleTxt"); rReceive (result, 2); summedValue += ((int)(result[0])) << 8 | ((int) result[1]); } return summedValue / sampleSize; } /** * @brief Read the Fluke's virtual light sensors. * Each sensor reports a total intensity in the left, middle, or right of the * Fluke's camera * Since readings of each brightness sensor can vary substantially (successive * readings may differ by 5000-10000), * each sensor can be queried sampleSize times and an average obtained. * @param brightSensors array to store intensity values * @param sampleSize how many readings are taken for each sensor * @pre space already allocated for brightSensors array * sampleSize > 0 * @post brightSensors[0] gives average value for left sensor * brightSensors[1] gives average value for middle sensor * brightSensors[2] gives average value for right sensor * Brightness values near 0 represent bright light * Brightness values may extend to about 65535 for a very dark region. */ void rGetBrightAll (int brightSensors[3], int sampleSize) { brightSensors[0] = rGetBrightTxt("left", sampleSize); brightSensors[1] = rGetBrightTxt("center", sampleSize); brightSensors[2] = rGetBrightTxt("right", sampleSize); } /** * @brief Reads one of the Fluke's virtual light sensors. * Each sensor reports a total intensity in the left, middle, or right of the * Fluke's camera * Since values of each obstacle sensor can vary substantially (successive * readings may differ by 5000-10000), * the sensor can be queried sampleSize times and an average obtained. * @param sensorName name of the bright sensor * @pre sensorName is "left", "center", "middle", or "right" * (not case sensitive) * designations "center" and "middle" are alternatives * for the same bright sensor * @param sampleSize how many readings are taken for the sensor * @pre sampleSize > 0 * @return reading from the specified bright sensor, averaged * over sampleSize number of data samples * Brightness values near 0 represent bright light * Brightness values may extend to about 65535 for a very dark region. */ int rGetBrightTxt (char * sensorName, int sampleSize) { int len = strlen(sensorName) + 1; char workingString [len]; int i; for (i = 0; i < len; i++) workingString[i] = tolower (sensorName[i]); char message[1]; unsigned char returned[2]; int summedValue = 0; if (strcmp(workingString, "left") == 0){ message[0] = 85; } else if (strcmp(workingString, "center") == 0){ message[0] = 86; } else if (strcmp(workingString, "middle") == 0){ message[0] = 86; } else if (strcmp(workingString, "right") == 0){ message[0] = 87; } else return -1; for(i=0; i