/* * * * * * * * * * * * * * * * * * * * * * * @file MyroC-movement.c * @brief Implementation file for Scribbler 2 movement 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 rTimedImageDisplay (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 #include // needed for usleep /* ************************* debugging variable ********************************* */ int timing_debug = 0; // 1 : print messages regarding timing; 0: no debug printing /* ************************ utility motion procedures ************************** */ /** * timer to stop a motion after specified time */ void * motion_timer_thread (void * delay_data) { /* make local copy of the delay data, so that subsequent processing does not * impact this timer */ motion_timing_data * local_delay_data = ((motion_timing_data *) delay_data); if (timing_debug) { printf ("starting motion timer for %lf\n", local_delay_data->duration_time); printf (" stored soc_num: %d\n", local_delay_data->robot_socket_num); printf (" actual soc_num: %d\n", socket_num); } /* time specified interval of delay */ long int utime = (long int)(1000000 * local_delay_data->duration_time); usleep (utime); /* if robot connection still active, and robot's motion command not superceded then stop robot */ if (timing_debug) { printf ("time expired for motion timer for %lf\n", local_delay_data->duration_time); } /* look up sequence number for this robot */ int i; for (i = 0; i < current_num_robots; i++) { if (robot_sock_array[i].robot_socket_num == socket_num) break; } if (i >= current_num_robots) { printf ("cannot stop motion: no robot associated with number %d\n", (int) socket_num); return (void *) 0; } if (timing_debug) { printf (" motion timer found: local seq number: %d\n", local_delay_data->motion_seq_num); printf (" stored seq number: %d\n", robot_sock_array[i].motion_seq_num); printf (" stored soc_num: %d\n", local_delay_data->robot_socket_num); printf (" actual soc_num: %d\n", socket_num); } /* check motion sequence number for this robot */ if (local_delay_data->motion_seq_num == robot_sock_array[i].motion_seq_num) { // printf ("stopping motion number %d\n", local_delay_data->motion_seq_num); /* store current socket number */ int temp_socket_num = socket_num; /* set socket number for this robot */ socket_num = robot_sock_array[i].robot_socket_num; if (timing_debug) { printf ("calling rStop\n"); } /* ensure only one message processed over Bluetooth at a time */ pthread_mutex_lock (&bluetooth_lock); /* stop robot motion */ char message[9]; message[0]= (unsigned char) 109; message[1]= (unsigned char) 100; // full stop value message[2]= (unsigned char) 100; message[3]= -1; message[4]= 0; message[5]= 0; message[6]= 0; message[7]= 0; message[8]= 0; /* any past motion timer is no longer relevent */ robot_sock_array[i].thread_motion_timer_id = 0; rSend_protected (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rMotors"); /* timer for this socket number no longer active */ robot_sock_array[i].thread_motion_timer_id = 0; if (timing_debug) { printf (" motion timer found: local seq number: %d\n", local_delay_data->motion_seq_num); printf (" stored seq number: %d\n", robot_sock_array[i].motion_seq_num); printf (" stored soc_num: %d\n", local_delay_data->robot_socket_num); printf (" actual soc_num: %d\n", socket_num); } /* restore previous socket number */ socket_num = temp_socket_num; pthread_mutex_unlock (&bluetooth_lock); } else { /* robot motion already superceded by later motion command */ printf ("motion for robot superceded by later motion\n"); } /* release lock ensuring only one message processed over Bluetooth at a time */ free (delay_data); /* pthread procedure must return void * data */ return (void *) 0; } /* duration-handling procedure for motion commands * once motion is started, motion duration based on parameter * * options: * duration > 0: processing blocks for given time (in seconds) * then processing resumes * duration < 0: timer thread is established * processing resumes immediately * after time expires, * if a new motion command has been issued for that robot * the timer is ignored * if no new motion command has been issued for that robot * the motion is stopped * duration = 0: processing continues with no subsequent timing * image replaced when new image created for this window * * win_index identifies index of window to be updated */ void process_motion_duration (double duration) { /* if duration > 0, * block further execution for given time */ // look up record number for this robot int robot_index; // index of robot in robot_sock_array for (robot_index = 0; robot_index < current_num_robots; robot_index++) { if (robot_sock_array[robot_index].robot_socket_num == socket_num) break; } if (robot_index >= current_num_robots) { printf ("cannot stop motion: no robot associated with number %d\n", (int) socket_num); return; } // printf ("duration processing: %10.8lf: ", duration); if (duration > 0) { // printf ("duration > 0\n"); int utime = (int)(duration * 1000000); usleep (utime); // send command to stop motion /* ensure only one message processed over Bluetooth at a time */ pthread_mutex_lock (&bluetooth_lock); /* stop robot motion */ char message[9]; message[0]= (unsigned char) 109; message[1]= (unsigned char) 100; // full stop value message[2]= (unsigned char) 100; message[3]= -1; message[4]= 0; message[5]= 0; message[6]= 0; message[7]= 0; message[8]= 0; /* any past motion timer is no longer relevent */ robot_sock_array[robot_index].thread_motion_timer_id = 0; rSend_protected (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rMotors"); /* release lock ensuring only one message processed over Bluetooth at a time */ pthread_mutex_unlock (&bluetooth_lock); } /* if duration < 0, * set timer in separate thread to hide window after designated time */ else if (duration < 0) { // printf ("duration < 0\n"); // create thread, packaging needed data into struct for timer motion_timing_data * delay_data = malloc (sizeof (motion_timing_data)); delay_data->duration_time = -duration; /* ensure socket data unchanged during timer setup */ pthread_mutex_lock (&bluetooth_lock); delay_data->robot_socket_num = socket_num; /* check sequence number for this motion with this robot */ delay_data->motion_seq_num = robot_sock_array [robot_index].motion_seq_num; /* release lock for finding sequence number */ pthread_mutex_unlock (&bluetooth_lock); /* create timer thread to end motion */ pthread_t thread; int err = pthread_create (&thread, NULL, &motion_timer_thread, delay_data); if (err != 0) { printf ("\ncannot create timer for non-blocking image;"); printf ("error code: %s\n", strerror(err)); } else { /* new motion timer is active */ robot_sock_array[robot_index].thread_motion_timer_id = thread; } } /* duration == 0: no processing needed */ else { /* any past motion timer is no longer relevent */ robot_sock_array[robot_index].thread_motion_timer_id = 0; // printf ("duration == 0\n"); } } /***********************************************************************/ /* 3. MOVEMENT - MOVEMENT - MOVEMENT - MOVEMENT - MOVEMENT - MOVEMENT */ /***********************************************************************/ /* Procedures in this section, except rMotors, include a "time" parameter. * * Throughout, "time" is interpreted as follows: * if time > 0, the call to the procedure is "blocking: * That is, a call to the procedure starts a motion, and * all other processing stops for the specified time (in seconds). * After the specified time interval, the motion stops, * and processing in the main program continues. * If time == 0, the procedure is "non-blocking": * That is, the procedure starts a motion, * and that motion continues until another motion command is issued. * Once the motion starts, other processing in the main * program continues. * If time < 0, the procedure is "non-blocking": * That is, a call to the procedure starts a motion, * and a timer is started for the abs("time"). * Once the time interval is completed, * if no other motion command has been issued, * a command to stop robot motion is given * if another motion command has been issued (before the * specified time), the new command is executed immediately, * and the timer is ignored. * For example, suppose rForward is called with a time of -5 seconds: * subsequent processing in the main program continues * if numerous beeps are issued or the camera is used or * a series of non-motion commands are issued, then * the rForward action stops in 5 seconds. * if an rRightTurn command is issued after 2 seconds, * then the rForward command is overridden by rRightTurn * and the timer for rForward (after another 3 seconds) * is ignored. * * Program termination with timed, non-blocking commands: * When a non-blocking time is specified (i.e., time < 0), processing within * the controlling program continues. However, the robot's Bluetooth connection * (established with rConnect) is not severed until the non-blocking timer has * expired and the motion stopped. In particular, rDisconnect blocks until * the specified non-blocking time is completed and the program tells the * robot to stop. */ /** * turn Scribbler left for a specified time and speed * @param speed the rate at which the robot should move left * linear range: -1.0 specifies right turn at full speed * 0.0 specifies no turn * 1.0 specifies left turn at full speed * @param time specifies the duration of the turn * if negative: robot continues to turn until given another * motion command or disconnected (non-blocking) * if nonnegative: robot turns for the given duration, in seconds */ void rTurnLeft (double speed, double time) { rMotors(speed,-speed); process_motion_duration (time); } /** * turn Scribbler right for a specified time and speed * @param speed the rate at which the robot should move right * linear range: -1.0 specifies left turn at full speed * 0.0 specifies no turn * 1.0 specifies right turn at full speed * @param time specifies the duration of the turn * if negative: robot continues to turn until given another * motion command or disconnected (non-blocking) * if nonnegative: robot turns for the given duration, in seconds */ void rTurnRight (double speed, double time) { rMotors(-speed,speed); process_motion_duration (time); } /** * turn Scribbler in direction for a specified time and speed * @param direction direction of turn, based on looking from * the center of the robot and facing forward * @param speed the rate at which the robot should move forward * linear range: -1.0 specifies turn at full speed * 0.0 specifies no turn * 1.0 specifies turn at full speed * @param time specifies the duration of the turn * if negative: robot continues to turn until given another * motion command or disconnected * if nonnegative: robot turns for the given duration, in seconds * @pre direction is "left" or "right", case insensitive */ void rTurnSpeed (char * direction, double speed, double time) { int len = strlen (direction)+1; char workingString [len]; int i; // Convert direction to lower case for (i = 0; i < len; i++) workingString[i] = tolower (direction[i]); if (strcmp(workingString, "left") == 0) { rTurnLeft(speed, time); } else if (strcmp(workingString, "right") == 0) { rTurnRight(speed, time); } else printf("error: rTurnSpeed takes either left or right. Received parameter: %s \n", direction); } /** * moves Scribbler forward for a specified time and speed * @param speed the rate at which the robot should move forward * 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 * @param time specifies the duration of movement * if negative: robot continues to move forward until given another * motion command or disconnected (non-blocking) * if nonnegative: robot moves forward for the given duration, in * seconds */ void rForward (double speed, double time) { rMotors(speed,speed); process_motion_duration (time); } /** * moves Scribbler forward at the largest possible speed for a specified time * @param time specifies the duration of movement * if negative: robot continues to move forward until given another * motion command or disconnected (non-blocking) * if nonnegative: robot moves forward for the given duration, in * seconds * @warning may take longer than usual to execute */ void rFastForward (double time) { char * forwardness = rGetForwardness(); if (forwardness[0] == 'f') { rForward(1.0, time); }// if was fluke-forward else { char message[9]; message[0]= 109; message[1]= (unsigned char) 255; message[2]= (unsigned char) 255; 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, "rFastForward"); process_motion_duration (time); 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) { rMotors(-speed,-speed); process_motion_duration (time); } /** * 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); /* prepare message to Scribbler */ 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"); /* log that another motion command has been issued for this robot */ /* ensure only one message processed over Bluetooth at a time */ pthread_mutex_lock (&bluetooth_lock); /* look up sequence number for this robot */ int i; for (i = 0; i < current_num_robots; i++) { if (robot_sock_array[i].robot_socket_num == socket_num) break; } if (i >= current_num_robots) { printf ("cannot stop motion: no robot associated with number %d\n", (int) socket_num); /* release lock ensuring only one message processed over Bluetooth at a time */ pthread_mutex_unlock (&bluetooth_lock); return ; } /* check motion sequence number for this robot */ robot_sock_array[i].motion_seq_num++; /* release lock ensuring only one message processed over Bluetooth at a time */ pthread_mutex_unlock (&bluetooth_lock); } /** * 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"); }