LoDrive.c

Go to the documentation of this file.
00001 /**
00002    \file  Robots/LoBot/irccm/LoDrive.c
00003    \brief Driving API for Robolocust iRobot Create Command Module control
00004    program.
00005 
00006    This file defines the functions that implement the driving API for a
00007    control program meant to be run on the iRobot Create's Command Module.
00008    These functions accept the high-level drive commands issued by the
00009    higher layers of the Robolocust controller and convert them to their
00010    equivalent Open Interface byte sequences.
00011 */
00012 
00013 /*
00014  ************************************************************************
00015  * The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005   *
00016  * by the University of Southern California (USC) and the iLab at USC.  *
00017  * See http://iLab.usc.edu for information about this project.          *
00018  *                                                                      *
00019  * Major portions of the iLab Neuromorphic Vision Toolkit are protected *
00020  * under the U.S. patent ``Computation of Intrinsic Perceptual Saliency *
00021  * in Visual Environments, and Applications'' by Christof Koch and      *
00022  * Laurent Itti, California Institute of Technology, 2001 (patent       *
00023  * pending; application number 09/912,225 filed July 23, 2001; see      *
00024  * http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     *
00025  ************************************************************************
00026  * This file is part of the iLab Neuromorphic Vision C++ Toolkit.       *
00027  *                                                                      *
00028  * The iLab Neuromorphic Vision C++ Toolkit is free software; you can   *
00029  * redistribute it and/or modify it under the terms of the GNU General  *
00030  * Public License as published by the Free Software Foundation; either  *
00031  * version 2 of the License, or (at your option) any later version.     *
00032  *                                                                      *
00033  * The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  *
00034  * that it will be useful, but WITHOUT ANY WARRANTY; without even the   *
00035  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      *
00036  * PURPOSE.  See the GNU General Public License for more details.       *
00037  *                                                                      *
00038  * You should have received a copy of the GNU General Public License    *
00039  * along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   *
00040  * to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   *
00041  * Boston, MA 02111-1307 USA.                                           *
00042  ************************************************************************
00043 */
00044 
00045 /*
00046    Primary maintainer for this file: mviswana usc edu
00047    $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/LoBot/irccm/LoDrive.c $
00048    $Id: LoDrive.c 13769 2010-08-08 01:34:02Z mviswana $
00049 */
00050 
00051 /*------------------------------ HEADERS ------------------------------*/
00052 
00053 // lobot headers
00054 #include "LoDrive.h"
00055 #include "LoSensors.h"
00056 #include "LoUtils.h"
00057 #include "LoIO.h"
00058 #include "LoTimer.h"
00059 #include "LoOpenInterface.h"
00060 
00061 // avr headers
00062 #include <avr/interrupt.h>
00063 
00064 // Standard C headers
00065 #include <stdlib.h>
00066 
00067 /*------------------------------ GLOBALS ------------------------------*/
00068 
00069 // The lobot controller's higher layers operate in terms of speed and
00070 // steering directions. The iRobot Create's motion primitives, however,
00071 // combine the speed and steering states into a single drive command
00072 // (with special values to indicate driving straight ahead and turning
00073 // in-place).
00074 //
00075 // Since the higher layers specify motor commands separately, we maintain
00076 // two independent variables to keep track of the current speed and turn
00077 // requests and update them as required based on the command sent in.
00078 //
00079 // Thus, when the high level issues turn commands, we update the turn
00080 // radius but leave the speed variable alone. Similarly, when drive
00081 // commands come in, we leave the turn radius as-is and update only the
00082 // speed variable. But both variables get combined when we need to
00083 // despatch the Open Interface drive command to the iRobot Create.
00084 static volatile int g_speed ;
00085 static volatile int g_radius = LOBOT_OI_DRIVE_STRAIGHT ;
00086 
00087 // These variables are used to implement the acceleration/decelartion
00088 // functionality asynchronously w.r.t. the main "thread."
00089 //
00090 // We need to have the speed ramping up/down operation performed
00091 // asynchronously to ensure that the main "thread," which is responsible
00092 // for listening and reacting to the robot's sensors, doesn't get caught
00093 // up in a long loop that can cause it to miss a potentially dangerous
00094 // situation (e.g., wheel drops or cliffs).
00095 //
00096 // We use the low-level controller's 10ms timer to achieve the desired
00097 // asynchrony. Every time this timer fires, we ramp the speed up or down
00098 // as required. As its name suggests, the g_target_speed variable is used
00099 // to specify the desired speed to which the robot must ramp up or down.
00100 // However, we need another variable (viz., g_ramp_flag) to indicate
00101 // whether or not speed ramping should even occur. This is important
00102 // because the drive module provides low-level functions that allow other
00103 // parts of the low-level controller to bypass the
00104 // acceleration/deceleration functionality (important, for instance, when
00105 // reacting to wheel drops or cliffs).
00106 static volatile char g_ramp_flag ;
00107 static volatile int  g_target_speed ;
00108 
00109 /*-------------------------- INITIALIZATION ---------------------------*/
00110 
00111 // Forward declaration
00112 static void accelerate_decelerate(void) ;
00113 
00114 // This function initializes the drive system's acceleration/deceleration
00115 // functionality by hooking into the 10ms timer's callback list.
00116 void lo_init_drive(void)
00117 {
00118    lo_add_timer10_cb(accelerate_decelerate) ;
00119 }
00120 
00121 /*--------------------- HIGH-LEVEL DRIVE COMMANDS ---------------------*/
00122 
00123 // Sometimes, the high level may not have any commands queued. At those
00124 // times, it should send a NOP to the low level. A NOP is exactly what
00125 // it says: a no/null operation. If the high level doesn't send a NOP or
00126 // sends some other unrecognized command (e.g., command code zero), the
00127 // low-level controller will stop the robot, which may not be what is
00128 // desired.
00129 void lo_nop(int param){}
00130 
00131 // Driving the robot forward
00132 void lo_forward(int speed)
00133 {
00134    lo_drive(lo_clamp(speed, 0, 500), g_radius, 1) ;
00135 }
00136 
00137 // Driving the robot backward
00138 void lo_reverse(int speed)
00139 {
00140    lo_drive(-lo_clamp(speed, 0, 500), g_radius, 1) ;
00141 }
00142 
00143 // Stopping the robot.
00144 //
00145 // DEVNOTE: The speed parameter is neither necessary nor utilized in this
00146 // function. However, because the main module despatches incoming
00147 // high-level commands using a map, we need to define all command
00148 // handlers as taking one parameter.
00149 void lo_stop(int speed)
00150 {
00151    lo_drive(0, LOBOT_OI_DRIVE_STRAIGHT, 1) ;
00152 }
00153 
00154 // Turning the robot left
00155 void lo_left(int turn_radius)
00156 {
00157    if (turn_radius < 100)
00158       turn_radius = LOBOT_OI_TURN_INPLACE_CCW ;
00159    else
00160       turn_radius = lo_clamp(turn_radius, 100, 2000) ;
00161    lo_drive(g_speed, turn_radius, 0) ;
00162 }
00163 
00164 // Turning the robot right
00165 void lo_right(int turn_radius)
00166 {
00167    if (turn_radius < 100)
00168       turn_radius = LOBOT_OI_TURN_INPLACE_CW ;
00169    else
00170       turn_radius = -lo_clamp(turn_radius, 100, 2000) ;
00171    lo_drive(g_speed, turn_radius, 0) ;
00172 }
00173 
00174 // Straightening the robot.
00175 //
00176 // DEVNOTE: The turn_radius parameter is neither necessary nor utilized
00177 // in this function. However, because the main module despatches incoming
00178 // high-level commands using a map, we need to define all command
00179 // handlers as taking one parameter.
00180 void lo_straight(int turn_radius)
00181 {
00182    lo_drive(g_speed, LOBOT_OI_DRIVE_STRAIGHT, 0) ;
00183 }
00184 
00185 // In-place turns
00186 void lo_cmd_spin(int angle)
00187 {
00188    // First, spin the robot by the requested amount and record the actual
00189    // amount of rotation.
00190    angle = lo_spin(225, lo_clamp(angle, -360, 360)) ;
00191 
00192    // Then, retrieve any unintended translational motion that occured
00193    // during the spin (can happen due to actuation errors).
00194    lo_wait(15) ;
00195    lo_tx(LOBOT_OI_CMD_QUERY_LIST) ;
00196    lo_tx(1) ;
00197    lo_tx(LOBOT_OI_SENSOR_DISTANCE) ;
00198    char buf[2] = {0} ;
00199    lo_rx(buf, 2, 20) ;
00200 
00201    // Finally, mark the actual amount of rotation and translation for the
00202    // next sensor packet so that high-level won't miss any crucial motion
00203    // updates.
00204    lo_update_pending_odometry(lo_make_word(buf[0], buf[1]), angle) ;
00205 }
00206 
00207 /*--------------------- LOW-LEVEL DRIVE COMMANDS ----------------------*/
00208 
00209 // Helper function to send specified drive params in terms of Open
00210 // Interface specs.
00211 static void drive_oi(int speed, int turn)
00212 {
00213    lo_tx(LOBOT_OI_CMD_DRIVE) ;
00214    lo_tx(lo_hibyte(speed)) ;
00215    lo_tx(lo_lobyte(speed)) ;
00216    lo_tx(lo_hibyte(turn))  ;
00217    lo_tx(lo_lobyte(turn))  ;
00218 }
00219 
00220 // This function takes care of accelerating or decelerating the robot
00221 // asynchronously w.r.t. the main "thread" to ensure that the robot
00222 // continues to listen and react to the sensors instead of getting tied
00223 // up in a long loop that ramps the speed up or down. It is invoked by
00224 // the 10ms timer's callback triggering mechanism.
00225 static void accelerate_decelerate(void)
00226 {
00227    if (g_ramp_flag)
00228    {
00229       if (g_speed < g_target_speed)
00230       {
00231          g_speed += 25 ;
00232          if (g_speed > g_target_speed)
00233             g_speed = g_target_speed ;
00234       }
00235       else if (g_speed > g_target_speed)
00236       {
00237          g_speed -= 25 ;
00238          if (g_speed < g_target_speed)
00239             g_speed = g_target_speed ;
00240       }
00241       else // target speed achieved
00242       {
00243          g_ramp_flag = 0 ;
00244          return ;
00245       }
00246       drive_oi(g_speed, g_radius) ;
00247    }
00248 }
00249 
00250 // Send Create drive commands in terms of speed and turn radius.
00251 //
00252 // DEVNOTE: To support the acceleration/deceleration functionality
00253 // without tying up the main "thread" in a long loop that can make the
00254 // robot deaf to its sensors while the speed ramps up/down, we use the
00255 // ATmega168's Timer2 ISR (via the low-level controller's 10ms
00256 // "generalized" timer) to perform the necessary speed ramping. The ISR
00257 // uses a flag that is set or cleared by this function to determine
00258 // whether or not perform the ramping operations.
00259 //
00260 // Additionally, the ISR uses and/or modifies some other variables
00261 // that are also used in the main "thread" (e.g., g_speed).
00262 //
00263 // To ensure that the variables shared by the main thread and the Timer2
00264 // ISR don't get mixed up, we need some sort of mutex mechanism.
00265 // Unfortunately, avr-libc doesn't provide such a beast. And rolling our
00266 // own is too much work. As a workaround, we simply disable the Timer2
00267 // interrupt prior to modifying the shared variables and reenable it once
00268 // we're done with the shared variables.
00269 void lo_drive(int speed, int turn_radius, char smooth)
00270 {
00271    if (g_speed == speed && g_radius == turn_radius)
00272       return ;
00273 
00274    g_radius = turn_radius ;
00275    if (smooth)
00276    {
00277       lo_suspend_timer10() ; // for atomicity, temporarily disable Timer2 int.
00278          g_ramp_flag = 1 ;
00279          g_target_speed = speed ;
00280       lo_resume_timer10()  ; // reenable Timer2 interrupt
00281    }
00282    else
00283    {
00284       lo_suspend_timer10() ; // for atomicity, temporarily disable Timer2 int.
00285          g_ramp_flag = 0 ;
00286       lo_resume_timer10()  ; // reenable Timer2 interrupt
00287       g_speed = speed ;
00288       drive_oi(g_speed, g_radius) ;
00289    }
00290 }
00291 
00292 // This function backs up the robot at the specified speed, moving it by
00293 // an amount at least equal to the specified distance (in mm). It returns
00294 // the actual distance backed up.
00295 int lo_backup(int speed, int distance)
00296 {
00297    // Start backing up
00298    lo_drive(speed, LOBOT_OI_DRIVE_STRAIGHT, 1) ;
00299 
00300    // Wait until the specified distance is reached
00301    int D = 0 ;
00302    while (abs(D) < abs(distance))
00303    {
00304       lo_wait(15) ;
00305 
00306       lo_tx(LOBOT_OI_CMD_QUERY_LIST) ;
00307       lo_tx(1) ;
00308       lo_tx(LOBOT_OI_SENSOR_DISTANCE) ;
00309 
00310       char buf[2] = {0} ;
00311       if (lo_rx(buf, 2, 20) >= 2)
00312          D += lo_make_word(buf[0], buf[1]) ;
00313    }
00314    lo_drive(0, LOBOT_OI_DRIVE_STRAIGHT, 1) ;
00315    return D ;
00316 }
00317 
00318 // Spin the robot at the specified speed by an amount equal to at least
00319 // the specified angle. Return the actual amount spun.
00320 int lo_spin(int speed, int angle)
00321 {
00322    // Start spinning
00323    int turn = (angle < 0) ? LOBOT_OI_TURN_INPLACE_CW
00324                           : LOBOT_OI_TURN_INPLACE_CCW ;
00325    lo_drive(speed, turn, 0) ;
00326 
00327    // Wait until the specified angle is reached
00328    int A = 0 ;
00329    while (abs(A) < abs(angle))
00330    {
00331       lo_wait(15) ;
00332 
00333       lo_tx(LOBOT_OI_CMD_QUERY_LIST) ;
00334       lo_tx(1) ;
00335       lo_tx(LOBOT_OI_SENSOR_ANGLE) ;
00336 
00337       char buf[2] = {0} ;
00338       if (lo_rx(buf, 2, 20) >= 2)
00339          A += lo_make_word(buf[0], buf[1]) ;
00340    }
00341    lo_drive(0, LOBOT_OI_DRIVE_STRAIGHT, 0) ;
00342    return A ;
00343 }
00344 
00345 /*---------------------------- DRIVE STATE ----------------------------*/
00346 
00347 // Is the robot stationary or moving?
00348 char lo_stopped(void)
00349 {
00350    return g_speed == 0 ;
00351 }
Generated on Sun May 8 08:41:30 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3