/* * * * * * * * * * * * * * * * * * * * * * * MyroC.h -- Header for a C-based, my-robot package for the Scribbler 2 * * Developers: * Creators: * Spencer Liberto * Dilan Ustek * Jordan Yuan * Henry M. Walker * Contributors: * Anita DeWitt * Jason Liu * Nick Knoebber * Vasilisa Bashlovkina * * * Based on a C++ package by April O'Neill, David Cowden, Dilan Ustek, * Erik Opavsky, and Henry M. Walker * * * * * * * * * * * * * * * * * * * * * * */ /* Index of all functions 0. LOW-LEVEL UTILITY rSend rReceive write_JPEG_file put_scanline_someplace my_error_exit read_JPEG_file rSetOpeningExchange rSetBluetoothEcho 1. GENERAL 2. SENSOR rConnect a. Scribbler Sensors rDisconnect rGetLightsAll rSetConnection rGetLightTxt rBeep rGetIRAll rBeep2 rGetIRTxt rSetName rGetLine rGetName rSetForwardness b. Fluke Sensors rGetForwardness rSetLEDFront rGetObstacleAll rSetLEDBack rGetObstacleTxt rGetBattery rGetBrightAll rGetStall rGetBrightTxt rSetIRPower 3. MOVEMENT 4. PICTURES rTurnLeft rTakePicture rTurnRight rSavePicture rTurnSpeed rLoadPicture rForward rDisplayPicture rFastForward rBackward rMotors rStop rHardStop 5. NOT IMPLEMENTED rGetAllSensors: format of 11 bytes of sensor data not known rGetInfo: format of block data from fluke not known */ /* Note: There is an r in the beginning of every function to make it easier to understand whether it is a robot function. */ #include "MyroC.h" #include #include #include #include #include #include #include #include #include #include #include #include #include // needed for waitpid #include // needed for waitpid #include #include #include /* version specification */ char * version = "MyroC.2.4"; int version_printed = 0; // 0 - version not yet printed; 1 - version printed /* flags to specify if Scribbler echoes a command (most or all do) */ #define ECHO_COMMAND_ON 1 #define ECHO_COMMAND_OFF 0 /* flags to specify if Scribbler echoes all sensor data */ #define ECHO_SENSOR_ON 1 #define ECHO_SENSOR_OFF 0 #define BYTES_OF_SENSOR_DATA 11 #define MAX(a,b) (((a) > (b)) ? (a) : (b)) #define MIN(a,b) (((a) < (b)) ? (a) : (b)) // baudrate for Bluetooth communication const int baudrate = 38400; // debug flag for communications int debug = 0; // 1 == echo all communications; 0 no extra printing // allow beeps and data exchange during opening connect int followOpeningExchange = 1; // 1 == robot beeps, exchanges data at connect; // 0 == no beeps or data exchange int socket_num; // current socket used for Bluetooth communications /*****************************************************************/ /* 0. UTILITY - UTILITY - UTILITY - UTILITY - UTILITY - UTILITY */ /*****************************************************************/ /* Sends commands via bluetooth*/ /** * send bytes to the Scribbler * @param message the command/data to be sent * @param numberOfBytes how many bytes to transmit * @param echo whether or not (1/0) the Scribbler echoes the command * @param sensor_echo whether or not the Scribbler sends all sensor data * back after echoing the command */ void rSend (char message[], int numberOfBytes, int echo, int sensor_echo, char * commandText) { /* send message */ int status = write (socket_num, message, numberOfBytes); //printf ("%d bytes written to socket\n", status); if (status != numberOfBytes) { perror ("Error in sending command over Bluetooth\n"); exit (1); } /* if Scribbler echoes command get echo, and check echo matches message */ if (echo) { int totalEchoed = 0; int errorPrinted = 0; while (totalEchoed 0) { /* bytes should echo correctly, except for bytes 0, 1, and 2 of the rMove command */ if ((!errorPrinted) && (echoedCommandChar != message[totalEchoed]) && ((message[0]!= 109) || /* not the rMove command */ ((message[0]== 109) && (totalEchoed > 2)))) { /* printf ("discrepency: "); */ fprintf (stderr, "Bluetooth communication error, "); fprintf (stderr, "starting at byte %d for %s command (code %d)\n", totalEchoed, commandText, message[0]); errorPrinted = 1; /* exit (1); */ } /* print 1 byte of data in %u format */ if (debug) { printf ("command echo '%3u' message '%3u'\n", echoedCommandChar & 255, message[totalEchoed] & 255); } totalEchoed += readNum; } } } if (sensor_echo) { int j = 0; while (j < BYTES_OF_SENSOR_DATA) { unsigned char echoedSensorChar = 255; /* read bytes of echoed sensor data */ j += read (socket_num, &echoedSensorChar, 1); if (debug && (echoedSensorChar != 255)) { printf ("sensor data '%3u'\n", echoedSensorChar & 255); echoedSensorChar = 255; } } } } /** * Receives commands via bluetooth */ void rReceive (unsigned char * message, int numberOfEchoBytes) { int i; for (i=0; i 0) { // printf("received '%u'\n", message[i]&255); i++; } } } /** * Convert a 2 dimensional array of JSAMPLE into a JPEG file * @param i_buffer array of JSAMPLE * @param i_height the hieght of the image * @param i_width the width of the image * @param outfile opened file for writing JPEG compression object * @param quality a measure of quality of the output image from 0 to 100 * * @pre i_buffer consists of sets of 3 consecutive JSAMPLEs, each being one pixel, * or an array of image_width * image_height pixel structs * * @credit this is a slightly modified version of write_JPEG_file(char *, int) * from example.c from the libjpeg.v8d library directory. * */ void write_JPEG_file (Picture pic, int i_height, int i_width, FILE * outfile, int quality) { JSAMPLE * rgb_array=malloc(sizeof(pic.pix_array)); int i,j; int index=0; for(i=0;ierr really points to a my_error_mgr struct, so coerce pointer */ my_error_ptr myerr = (my_error_ptr) cinfo->err; /* Always display the message. */ /* We could postpone this until after returning, if we chose. */ (*cinfo->err->output_message) (cinfo); /* Return control to the setjmp point */ longjmp(myerr->setjmp_buffer, 1); } JSAMPLE * read_JPEG_file (char * filename, int * i_height, int * i_width) { /* This struct contains the JPEG decompression parameters and pointers to * working space (which is allocated as needed by the JPEG library). */ struct jpeg_decompress_struct cinfo; /* We use our private extension JPEG error handler. * Note that this struct must live as long as the main JPEG parameter * struct, to avoid dangling-pointer problems. */ struct my_error_mgr jerr; FILE * infile; /* source file */ JSAMPARRAY buffer; /* Output row buffer */ int row_stride; /* physical row width in output buffer */ /* In this example we want to open the input file before doing anything else, * so that the setjmp() error recovery below can assume the file is open. * VERY IMPORTANT: use "b" option to fopen() if you are on a machine that * requires it in order to read binary files. */ /* Open the file */ if ((infile = fopen(filename, "rb")) == NULL) { fprintf(stderr, "can't open %s\n", filename); return NULL; } /* Step 1: allocate and initialize JPEG decompression object */ // First deal with error handlers /* We set up the normal JPEG error routines, then override error_exit. */ cinfo.err = jpeg_std_error(&jerr.pub); jerr.pub.error_exit = my_error_exit; /* Establish the setjmp return context for my_error_exit to use. */ if (setjmp(jerr.setjmp_buffer)) { /* If we get here, the JPEG code has signaled an error. * We need to clean up the JPEG object, close the input file, and return. */ jpeg_destroy_decompress(&cinfo); fclose(infile); return NULL; } /* Now we can initialize the JPEG decompression object. */ jpeg_create_decompress(&cinfo); /* Step 2: specify data source (eg, a file) */ jpeg_stdio_src(&cinfo, infile); /* Step 3: read file parameters with jpeg_read_header() */ (void) jpeg_read_header(&cinfo, TRUE); /* We can ignore the return value from jpeg_read_header since * (a) suspension is not possible with the stdio data source, and * (b) we passed TRUE to reject a tables-only JPEG file as an error. * See libjpeg.txt for more info. */ /* Step 4: Start decompressor */ (void) jpeg_start_decompress(&cinfo); *i_height = cinfo.output_height; *i_width = cinfo.output_width; JSAMPLE * i_buffer = malloc(sizeof(JSAMPLE) * (*i_height) * (*i_width) * 3); /* We may need to do some setup of our own at this point before reading * the data. After jpeg_start_decompress() we have the correct scaled * output image dimensions available, as well as the output colormap * if we asked for color quantization. * In this example, we need to make an output work buffer of the right size. */ /* JSAMPLEs per row in output buffer */ row_stride = cinfo.output_width * cinfo.output_components; /* Make a one-row-high sample array that will go away when done with image */ buffer = (*cinfo.mem->alloc_sarray) ((j_common_ptr) &cinfo, JPOOL_IMAGE, row_stride, 1); /* Step 5: while (scan lines remain to be read) */ /* jpeg_read_scanlines(...); */ /* Here we use the library's state variable cinfo.output_scanline as the * loop counter, so that we don't have to keep track ourselves. */ int index_in_i_buffer = 0; while (cinfo.output_scanline < cinfo.output_height) { /* jpeg_read_scanlines expects an array of pointers to scanlines. * Here the array is only one element long, but you could ask for * more than one scanline at a time if that's more convenient. */ (void) jpeg_read_scanlines(&cinfo, buffer, 1); /* Assume put_scanline_someplace wants a pointer and sample count. */ put_scanline_someplace(i_buffer, buffer[0], row_stride, &index_in_i_buffer); } /* Step 7: Finish decompression */ (void) jpeg_finish_decompress(&cinfo); /* We can ignore the return value since suspension is not possible * with the stdio data source. */ /* Step 8: Release JPEG decompression object */ /* This is an important step since it will release a good deal of memory. */ jpeg_destroy_decompress(&cinfo); /* After finish_decompress, we can close the input file. * Here we postpone it until after no more JPEG errors are possible, * so as to simplify the setjmp error logic above. (Actually, I don't * think that jpeg_destroy can do an error exit, but why assume anything...) */ fclose(infile); /* At this point you may want to check to see whether any corrupt-data * warnings occurred (test whether jerr.pub.num_warnings is nonzero).*/ /* And we're done! */ return i_buffer; } /** * @brief Turn on and off opening exchange with robot, including * distinctive beeps when connecting * retrieval of robot name * @param onOff char 'y' enables beeps (default) * char 'n' disables beeps * other character values ignored */ void rSetOpeningExchange (char onOff) { if (tolower (onOff) == 'y') followOpeningExchange = 1; else if (tolower (onOff) == 'n') followOpeningExchange = 0; else printf ("invalid parameter for enabling/disabling rconnect beeps\n"); } /*****************************************************************/ /* 1. GENERAL - GENERAL - GENERAL - GENERAL - GENERAL - GENERAL */ /*****************************************************************/ /** * connects program to Scribbler * @param address string, giving name of workstation port * or a Scribbler Bluetooth designation * * several string formats are possible * a communications port, such as "/dev/rfcomm0" * a MAC address, such as "00:1E:19:01:0E:13" * a Scribbler 2 fluke serial number, such as "245787" * a full IPRE serial number, such as "IPRE245787" * a Fluke 2 serial number (hexadecimal), such as "021F" * a full Fluke 2 serial number, such as "Fluke2-021F" * * @return the socket number of communications port * * @post subsequent communications will take place through * this socket, unless changed by rSetConnection */ int rConnect (const char * address) { if (strchr(address, '/') > 0) { /* communications port identified */ socket_num = open (address, O_RDWR | O_NOCTTY); if (socket_num < 0) { fprintf (stderr, "error opening communication channel %s\n", address); exit (1); } } else { /* if fluke or IPRE or Fluke 2 serial number, find MAC address */ char fluke_num [7] = {0}; char mac_addr [18] = {0}; int endpos, i; /* if a fluke/IPRE number, last 6 characters will be digits string before digits either is empty or starts with IPRE if a fluke2 number, then last 4 characters will be hex digits string before digits either is empty or starts with Fluke2- */ /* first trim whitespace at end */ endpos = strlen (address)-1; while (endpos >= 0 && isspace (address[endpos])) { endpos--; } /* determine if address is valid Fluke designator */ /* if so, serial number stored in fluek_num array */ int isValid = 0; /* first check if IPRE number (i.e., Fluke 1) last 6 characters must be decimal digits before number may be empty or contain "IPRE" */ /* copy last 6 characters, if digits */ for (i = 0; i < 6 && isdigit (address[endpos - 5 + i]); i++) fluke_num [i] = address[endpos - 5 + i]; if (strlen(fluke_num) == 6 && (endpos == 5 || strncmp ("IPRE", address, 4) == 0)) isValid = 1; // printf ("test for IPRE number, result = %d\n", isValid); // printf (" fluke_num: %s\n", fluke_num); if (!isValid) { for (i = 0; i < 4 && isxdigit (address[endpos - 3 + i]); i++) fluke_num [i] = address[endpos - 3 + i]; fluke_num [4] = fluke_num [5] = 0; if (strlen(fluke_num) == 4 && (endpos == 3 || strncmp ("Fluke", address, 5) == 0)) isValid = 1; } // printf ("test for Fluke 2 number, result = %d\n", isValid); // printf (" fluke_num: %s\n", fluke_num); /* now have an IPRE or Fluke 2 serial number so look up MAC address */ if (isValid) { /* code segment closely pattered from simplescan.c program from Bluethooth Essentials for Programmers by Huang & Rudolph */ printf ("Using Bluetooth to find connection address for %s\n", address); inquiry_info *devices = NULL; int max_rsp, num_rsp; int adapter_id, sock, len, flags; int i; char addr[19] = { 0 }; char name[248] = { 0 }; adapter_id = hci_get_route(NULL); sock = hci_open_dev( adapter_id ); //open socket if (adapter_id < 0 || sock < 0) { perror("error opening socket"); exit(1); } // initializations and memory allocation len = 8; max_rsp = 255; flags = IREQ_CACHE_FLUSH; devices = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info)); num_rsp = hci_inquiry(adapter_id, len, max_rsp, NULL, &devices, flags); //inquire the number of responding devices // printf ("number responses: %d\n", num_rsp); if( num_rsp < 0 ) { perror("error in Bluetoon hci_inquiry\n"); exit (1); } // print name and address of each visible device for (i = 0; i < num_rsp; i++) { ba2str(&(devices+i)->bdaddr, addr); //turn address of the device into string memset(name, 0, sizeof(name)); hci_read_remote_name(sock, &(devices+i)->bdaddr, sizeof(name), name, 0); //printf("%s %s\n", addr, name); if (strstr (name, fluke_num) > 0) { strcpy (mac_addr, addr); break; } } // printf ("mac address found: %s\n", mac_addr); //clean up free( devices ); close( sock ); //addr has MAC address, if Bluetooth name found if (strlen (fluke_num) == 0) { fprintf (stderr, "Scribbler serial number %s not available\n", address); exit (1); } } /* check possible machine address, if >= 2 colons in address */ if ((strchr (address, ':') > 0) && (strchr (address, ':') != strrchr (address, ':'))) strcpy (mac_addr, address); /* cannot connect if MAC address not found */ if (strlen (mac_addr) == 0) { fprintf (stderr, "cannot find Bluetooth channel for %s\n", address); exit (1); } /* MAC address found, so connection seems possible */ printf (" Machine (MAC) address for %s is %s\n", fluke_num, mac_addr); struct sockaddr_rc addr = { 0 }; int status; // allocate a socket socket_num = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); // set the connection parameters (who to connect to) addr.rc_family = AF_BLUETOOTH; addr.rc_channel = 1; str2ba( mac_addr, &addr.rc_bdaddr ); // connect to server status = connect(socket_num, (struct sockaddr *)&addr, sizeof(addr)); if (status < 0) { perror ("error in establishing a connection\n"); exit (1); } } // set terminal interface to control asynchronous communications ports struct termios io; tcgetattr(socket_num, &io); cfsetospeed(&io, baudrate); cfsetispeed(&io, baudrate); cfmakeraw(&io); tcsetattr(socket_num, TCSANOW, &io); tcsetattr(socket_num, TCSAFLUSH, &io); // flush input and output tcflush(socket_num, TCIOFLUSH); /* sound distinctive opening tones */ if (followOpeningExchange) { rBeep(.03, 784); rBeep(.03, 880); rBeep(.03, 698); rBeep(.03, 349); rBeep(.03, 523); /* print greeting at terminal */ if (version_printed == 0) { version_printed = 1; printf ("Hello, I'm %s, running %s\n", rGetName(), version); } else { printf ("Hello, I'm %s, running %s\n", rGetName()); } printf (" connection established to %s\n", address); printf (" "); // indent forwardness message first time rSetForwardness("scribbler-forward"); } return socket_num; } /** * disconnect program from Scribbler */ void rDisconnect() { close (socket_num); } /** * set current connection to the socket number * @param new_socket_num the number of an open socket for communication * @pre new_socket_num has been returned by rConnect * the designated socket has not been closed */ void rSetConnection (int new_socket_num) { socket_num = new_socket_num; } /** * Beeps with the given duration and frequency * @param duration length of note in seconds * @param frequency frequency of pitch in cycles per second (hertz) * @pre duration > 0.0 */ void rBeep(double duration, int frequency) { char message[9]; message[0]=113; // set speaker message[1]= ((int)(duration * 1000)) >> 8; // high-order 8 bits message[2]= ((int)(duration * 1000)) % 256; // low-order 8 bits message[3]= frequency >> 8; // high-order 8 bits message[4]= frequency % 256;// low-order 8 bits message[5]= -1 ; // end of data set message[6]= 0; // not used message[7]= 0; // not used message[8]= 0; // not used rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rBeep"); } /** * Generates two notes for the prescribed duration * @param duration length of note in seconds * @param freq1 frequency of first pitch in cycles per second (hertz) * @param freq2 frequency of second pitch in cycles per second (hertz) * @pre duration > 0.0 */ void rBeep2(double duration, int freq1, int freq2) { char message[9]; message[0]=114; // set speaker message[1]= ((int)(duration * 1000)) >> 8; // high-order 8 bits message[2]= ((int)(duration * 1000)) % 256; // low-order 8 bits message[3]= freq1 >> 8; // high-order 8 bits message[4]= freq1 % 256; // low-order 8 bits message[5]= freq2 >> 8 ; // high-order 8 bits message[6]= freq2 % 256; // low-order 8 bits message[7]= -1; // end of data set message[8]= 0; // not used rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rBeep2"); } /** * Change name stored in the robot to the 16-byte name given * @param name specifies new name of robot * if < 16 bytes given, name is filled with null characters * if >= 16 bytes given, name is truncated to 15 bytes plus null */ void rSetName (const char * name) { char message[9]; int strlength = strlen(name); int i; message[0]= 110; // set first 8 bytes of the robot's 16-byte name for (i = 0; i < 8; i++) if (i < strlength) message [i+1] = name[i]; else message [i+1] = 0; //first part of scribbler name being set rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rSetName (bytes 0-7)"); // printf ("first 8 bytes set\n"); message[0]= 119; // set first 8 bytes of the robot's 16-byte name for (i = 8; i < 16; i++) if (i < strlength) message [i-7] = name[i]; else message [i-7] = 0; //last part pf scribbler name being set rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rSetName (bytes 8-15)"); // printf ("second 8 bytes set\n"); } /** * @return information about the name of the robot * @post the returned name is a newly-allocated 17-byte string */ const char * rGetName () { char message[9]; unsigned char * newName = (unsigned char *) malloc(sizeof(char)*17); //Message 78 recieves 9 bytes of echo, and 8 bytes of message. message[0]= 78; // get first 8 bytes of the robot's 16-byte name 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 rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetName (bytes 0-7)"); rReceive (newName, 8); newName[8] = '\0'; //printf("%s\n", newName); message[0]= 64; // get last 8 bytes of the robot's 16-byte name 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 rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetName (bytes 8-15)"); rReceive (newName+8, 8); newName[16] = '\0'; return (char *) newName; } /** * specifies which end of the Scribbler is considered the front * @param direction identifies front direction * @pre direction is either "fluke-forward" or "scribbler-forward" * (not case sensitive) */ void rSetForwardness (char * direction) { int len = strlen(direction) + 1; char workingString [len]; int i; for (i = 0; i < len; i++) workingString[i] = tolower (direction[i]); char message[2]; message[0]=128; if (strcmp(workingString, "fluke-forward") == 0) { /* code for fluke forward is 0x80; */ message[1] = (unsigned char) 0x80; printf ("setting fluke forward\n"); } else if (strcmp (workingString, "scribbler-forward") == 0) { /* code for Scribbler forward is 0 */ message[1] = (unsigned char) 0; printf ("setting scribbler forward\n"); } else { fprintf (stderr, "rSetForwardness: unknown direction. Give either fluke-forward or scribbler-forward"); return; } rSend (message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetForwardness"); } /** * alternative to rSetForwardness for compatibility with earlier MyroC */ void rSetForwardnessTxt (char * direction) { rSetForwardness (direction); } /** * Gets the forwardness of the Scribbler * @return either "fluke-forward" or "scribbler-forward" */ char * rGetForwardness () { char message[5]; char * result; message[0]= 90;//read_mem(0,0) message[1]= 0; //page (2 bytes) message[2]= 0; message[3]= 0; //offset (2 bytes); message[4]= 0; rSend (message, 5, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rGetForwardness"); unsigned char returned = 7; rReceive (&returned, 1); char flag = (unsigned char) 0xDF; //printf("rGetForwardness %x\n", returned); if( returned == flag) result = "scribbler-forward"; else result = "fluke-forward"; return result; } /** * Set the front [fluke] LED on or off * @param led value 1 turns on LED * value 0 turns off LED * @pre led must be 0 or 1 */ void rSetLEDFront(int led){ char message[1]; if( led >= 1) { message[0]= 116; // turn the led on rSend(message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "Turn Front LED ON"); } else { message[0]= 117; // turn the led off rSend(message,1,ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "Turn Front LED OFF"); } } /** * Set the the intensity of the back fluke LED, * @param led intensity of the LED * values between 0 and 1 provide a range of brightness * from off to full intensity * values bigger than 1 are treated as 1 (full brightness) * values less than 0 are treated as 0 (LED off). */ void rSetLEDBack(double led) { char value = 0; if (led >= 1.0) { value = 255; } else if (led >= 0.0 && led < 1.0 ) { // convert led into an int for message // scribbler does not display light for values below 140. // Use a linear scale between 140-255 value = (char) (led * 115.0 + 140.0); } else if (led < 0.0) value = 0; char message[2]; message[0]= 126; // turn the leds on message[1]= value; // testing int rSend(message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetLEDBack"); } /** * Get the percentage of volts left in the batteries of the scribbler * @warning may not return useful values * @return percentage of battery voltage */ double rGetBattery() { char message[1]; unsigned char receivedMessage[2]; message[0]= 89; // battery infomation // message[1]= (int) message[1] << 8 || (int) message[0] ; // not used rSend(message, 1 ,ECHO_COMMAND_OFF, ECHO_SENSOR_OFF , "rGetBattery"); rReceive (receivedMessage, 2); int battery = (int)receivedMessage[0] << 8 ||(int) receivedMessage[1]; //printf("%d",battery); //Percentage is volts in robot/total voltage return (battery/20.9813)*100; } /** * Determine if robot has stalled * Since readings of each brightness sensor can vary substantially, * each sensor can be queried sampleSize times and an average obtained. * @param sampleSize how many readings are taken for each sensor * @pre sampleSize > 0 * @return whether or not robot current has stalled * @post Returns 1 if the robot has stalled * Returns 0 otherwise. */ int rGetStall (int sampleSize) { char message[9]; unsigned char receivedMessage[2]; int summedValue; message[0]= 79; message[1]= 0; message[2]= 0; message[3]= 0; message[4]= 0; message[5]= 0; message[6]= 0; message[7]= 0; message[8]= 0; int i; for(i=0; i 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; } /** * 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; } /** * 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)); } /** * 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")){ message[0]= 72; } else if (strcmp(workingString, "right")){ message[0]= 71; } 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)); } /** * Use Scribbler 2 line sensors of Scribbler to check for a black line * on a white 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 * @post for each irSensors array value * return 0 indicates line is identified * return 1 indicates line is not identified */ 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 */ /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ /** * 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"); } /** * 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 * Obstacle values near 0 represent no obstacle is seen * Obstacle values may approach 6000 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); } /** * 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 * Obstacle values near 0 represent no obstacle is seen * Obstacle values may approach 6000 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.0; 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; } /** * Read the Fluke's virtual light sensors. * 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); } /** * Reads one of the Fluke's virtual light sensors. * 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 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 sampleSize > 0 * @return reading from the specified obstacle 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")){ message[0] = 85; } else if (strcmp(workingString, "center")){ message[0] = 86; } else if (strcmp(workingString, "right")){ message[0] = 87; } else return -1; for(i=0; i= 0) { int utime = (int)(time * 1000000); usleep(utime); rMotors(0.0,0.0); }// if blocking command, wait and stop rSetForwardness(forwardness); } //else scribbler-forward } /** * moves Scribbler backward for a specified time and speed * @param speed the rate at which the robot should move backward * linear range: -1.0 specifies move forward at full speed * 0.0 specifies no forward/backward movement * 1.0 specifies move backward at full speed * @param time specifies the duration of the turn * if negative: robot continues to go backward until given another * motion command or disconnected (non-blocking) * if nonnegative: robot moves backward for the given duration, in * seconds * */ void rBackward (double speed, double time) { if(time < 0) rMotors(speed*-1,speed*-1); else { int utime = (int)(time * 1000000); speed = speed * (-1.0); rMotors(speed,speed); usleep(utime); rMotors(0.0,0.0); } } /** * move robot with given speeds for the left and right motors * continues until given another motion command or disconnected (non-blocking) * @param leftSpeed the rate at which the left wheel should turn * linear range: -1.0 specifies move backward at full speed * 0.0 specifies no forward/backward movement * 1.0 specifies move forwardward at full speed * @param rightSpeed the rate at which the right wheel should turn * linear range: -1.0 specifies move backward at full speed * 0.0 specifies no forward/backward movement * 1.0 specifies move forward at full speed */ void rMotors (double leftSpeed, double rightSpeed) { /* Ensure correct range - from -1 to 1 */ leftSpeed = MIN(leftSpeed, 1.0); leftSpeed = MAX(leftSpeed, -1.0); rightSpeed = MIN(rightSpeed, 1.0); rightSpeed = MAX(rightSpeed, -1.0); int lP = (int)((leftSpeed + 1.0) * 100); int rP = (int)((rightSpeed + 1.0) * 100); char message[9]; message[0]= 109; message[1]= (unsigned char) lP; message[2]= (unsigned char) rP; message[3]= -1; message[4]= 0; message[5]= 0; message[6]= 0; message[7]= 0; message[8]= 0; rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rMotors"); } /** * directs robot to stop movement */ void rStop() { rMotors(0.0,0.0); } /** * cuts power to the motor of the robot */ void rHardStop() { /* send hard stop command to robot */ char message[9]; message[0]= 108; message[1]= -1; message[2]= 0; message[3]= 0; message[4]= 0; message[5]= 0; message[6]= 0; message[7]= 0; message[8]= 0; rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rHardStop"); } /***********************************************************************/ /* 4.) PICTURES PICTURES PICTURES PICTURES PICTURES PICTURES PICTURES */ /***********************************************************************/ /* In this section we deal with taking and manipulating pictures produced from the Scribbler 2 camera. These pictures will always be 192x256 (192 rows by 256 columns) and internally are defined as type Picture, which hold a 2D array of type of Pixel. These can be saved and loaded as .jpeg files. */ /** * Use the camera to take a photo * @return Picture */ Picture rTakePicture() { int height = 192; //picture will always be these dimensions int width = 256; int size = width*height; Picture result; result.width = width; result.height = height; Pixel pix; unsigned char * yuv_buffer = (unsigned char*)malloc(sizeof(unsigned char) * size); char message[1]; message[0]=83; //83 is the command to take a picture; rSend(message,1,ECHO_COMMAND_OFF,ECHO_SENSOR_OFF, "rTakePicture"); //does not need echos rReceive(yuv_buffer,size); //recieve the YUV data into the yuv_buffer /*most of the below code comes from Scibbler.CPP Changes made: syntax so that it would work in C, and modifications so that it would return a Picture. */ //Warning: details of YUV encoding may seem obscure //"Black Voodoo Magic, since I don't know the YUV encode format." - quote from anonymous developer int vy, vu, y1v, y1u, uy, uv, y2u, y2v; int Y = 0,U = 0,V = 0; int i,j; for(i = 0; i < height; i++) { for(j = 0; j < width; j++) { if( j >= 3 ) { vy = -1; vu = -2; y1v = -1; y1u = -3; uy = -1; uv = -2; y2u = -1; y2v = -3; } else { vy = 1; vu = 2; y1v = 3; y1u = 1; uy = 1; uv = 2; y2u = 3; y2v = 1; } //VYUY VYUY VYUY if( j % 4 == 0 ) { V = (int)yuv_buffer[i * width + j]; Y = (int)yuv_buffer[i * width + j + vy]; U = (int)yuv_buffer[i * width + j + vu]; } if( j % 4 == 1 ) { Y = (int)yuv_buffer[i * width + j]; V = (int)yuv_buffer[i * width + j + y1v]; U = (int)yuv_buffer[i * width + j + y1u]; } if( j % 4 == 2 ) { U = (int)yuv_buffer[i * width + j]; Y = (int)yuv_buffer[i * width + j + uy]; V = (int)yuv_buffer[i * width + j + uv]; } if( j % 4 == 3 ) { Y = (int)yuv_buffer[i * width + j]; U = (int)yuv_buffer[i * width + j + y2u]; V = (int)yuv_buffer[i * width + j + y2v]; } U = U - 128; V = V - 128; double R =MAX(MIN(Y + 1.13983 * V, 255.0), 0.0); double G =MAX(MIN(Y - 0.39466 * U - 0.7169 * V, 255.0),0.0); double B =MAX(MIN(Y + 2.03211 * U, 255.0), 0.0); pix.R=(unsigned char)R; pix.G=(unsigned char)G; pix.B=(unsigned char)B; result.pix_array[i][j]=pix; //put the pixel into the pix array. } } free(yuv_buffer); //no longer need yuv_buffer return result; } /** * Save a Picture to a .jpeg * @param pic RGB picture struct from Scribbler 2 camera * @param filename the name of the file * @pre filename ends with .jpeg or .jpg. * @post If the file does not exist, a new file will be created. * @post If the file exists, the file will be overwritten. */ void rSavePicture(Picture pic, char * filename) { FILE * outfile; if ((outfile = fopen(filename, "wb")) == NULL) { fprintf(stderr, "can't open %s\n", filename); exit(1); } write_JPEG_file (pic, 192, 256, outfile, 100); } /** * Load a picture from a .jpeg file. * @param filename the name of the file * @pre file must exist * @pre file must be a 256x192 .jpeg or .jpg * @return Picture */ Picture rLoadPicture(char * filename) { int width = 256; int height = 192; Picture to_return; to_return.height = height; to_return.width = width; JSAMPLE * temp; // contains unsigned chars, not pixels temp = read_JPEG_file (filename, &height, &width); int i, j; int temp_index = 0; for (i = 0; i < height; i++) for (j = 0; j < width; j++) { to_return.pix_array[i][j].R = temp[temp_index++]; to_return.pix_array[i][j].G = temp[temp_index++]; to_return.pix_array[i][j].B = temp[temp_index++]; } free(temp);// deallocate temp return to_return; } /** * Display a picture in a new window * @param pic RGB picture struct from Scribbler 2 camera * @param duration if duration > 0, operation is blocking * if duration <= 0, operation is non-blocking * for duration != 0, picture displayed for abs(duration) * seconds or until picture closed manually * if duration == 0, picture displayed until closed manually * @param windowTitle The title of the window that appears. * white spaces will be replaced with underscores. * @pre windowTitle is less than 100 characters. */ void rDisplayPicture(Picture pic, double duration, const char * windowTitle) { /* set up child process to display image from stdin*/ int fd[2]; /* provide file descriptor pointer array for pipe */ /* within pipe: fd[0] will be input end fd[1] will be output end */ if (pipe (fd) < 0) /* create pipe and check for an error */ { perror("pipe error"); exit (1); } pid_t viewer_pid; if ((viewer_pid = fork()) < 0) /* apply fork and check for error */ { perror ("error in fork"); exit (1); } if (0 == viewer_pid) { /* child process will handle viewer */ close (fd[1]); /* close output end, leaving input open */ /* set standard input to pipe */ if (fd[0] != STDIN_FILENO) { if (dup2(fd[0], STDIN_FILENO) != STDIN_FILENO) { perror("dup2 error for standard input"); exit(1); } close(fd[0]); /* not needed after dup2 */ } /* set up title parameter, allowing up to 100 characters for title */ int len = strlen(windowTitle) + 1; char titlestr [len]; int i; /* replace whitespace in window title with underscores */ for (i = 0; (i < len); i++) { if (isspace(windowTitle[i])) titlestr[i] = '_'; else titlestr[i] = windowTitle[i]; } execlp ("display", "display", "-title", titlestr, (char *) 0); exit (0); } /* parent process continues with robot control */ close(fd[0]); FILE * outfile = fdopen (fd[1], "w"); write_JPEG_file (pic, 192, 256, outfile, 100); /* set up process to time image display and kill image process when done */ if (duration != 0) { pid_t timer_pid; /* variable to record process id of child */ timer_pid = fork(); /* create new process */ if ( -1 == timer_pid) /* check for error in spawning child process */ { perror ("error in fork"); exit (1); } if (0 == timer_pid) /* timing process */ { int utime = (int)(abs(duration) * 1000000); usleep(utime); /* after time expired, kill viewer process */ kill (viewer_pid, SIGKILL); /* timing process completed */ exit (0); } /* if display command blocking, wait until timing process completed */ if (duration > 0) { waitpid (timer_pid, NULL, 0); } } }