test-pwiibot.C

00001 /*!@file AppDevices/test-wiimote.C Test the wiimote */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/PWiiBot/test-pwiibot.C $
00035 // $Id: test-pwiibot.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "Component/ModelManager.H"
00039 #include "Devices/DeviceOpts.H"
00040 #include "Image/Image.H"
00041 #include "Image/DrawOps.H"
00042 #include "Media/FrameSeries.H"
00043 #include "Transport/FrameInfo.H"
00044 #include "Raster/GenericFrame.H"
00045 #include "Raster/GenericFrame.H"
00046 #include "Media/FrameSeries.H"
00047 #include "Transport/FrameInfo.H"
00048 #include "GUI/ImageDisplayStream.H"
00049 #include "GUI/XWinManaged.H"
00050 #include "GUI/GeneralGUI.H"
00051 #include "Robots/PWiiBot/PWiiController.H"
00052 #include "Raster/Raster.H"
00053 
00054 #include <fcntl.h>
00055 #include <sys/types.h>
00056 #include <sys/stat.h>
00057 #include <signal.h>
00058 
00059 /*
00060 #ifdef HAVE_LIBWIIMOTE
00061 extern "C" {
00062 #define _ENABLE_TILT
00063 #define _ENABLE_FORCE
00064 #include <wiimote.h>
00065 #include <wiimote_api.h>
00066 }
00067 #endif*/
00068 
00069 #define KEY_UP 98
00070 #define KEY_DOWN 104
00071 #define KEY_LEFT 100
00072 #define KEY_RIGHT 102
00073 
00074 
00075 
00076 
00077 /* GUI STUFF */
00078 
00079 void setupGUI(nub::soft_ref<GeneralGUI> pwiiGUI, nub::soft_ref<OutputFrameSeries> ofs, nub::soft_ref<PWiiController> controller) {
00080 
00081   Image<PixRGB<byte> > img(1,1,ZEROS);
00082   ofs->writeRGB(img, "Output", FrameInfo("output", SRC_POS));
00083 
00084   pwiiGUI->startThread(ofs);
00085   pwiiGUI->setupGUI(controller.get(), true);
00086 
00087   //Setup Meters
00088   pwiiGUI->addMeter(controller->getMotor1SpeedPtr(),
00089         "Motor 1 Speed", 100, PixRGB<byte>(255, 0, 0));
00090   pwiiGUI->addMeter(controller->getMotor2SpeedPtr(),
00091         "Motor 2 Speed", 100, PixRGB<byte>(255, 0, 0));
00092   pwiiGUI->addMeter(controller->getMotor1DirPtr(),
00093         "Motor 1 Direction", 4, PixRGB<byte>(192, 255, 0));
00094   pwiiGUI->addMeter(controller->getMotor2DirPtr(),
00095         "Motor 2 Direction", 4, PixRGB<byte>(192, 255, 0));
00096   pwiiGUI->addMeter(controller->getTransVelPtr(),
00097         "Translational Velocity", 100, PixRGB<byte>(192, 255, 0));
00098   pwiiGUI->addMeter(controller->getRotVelPtr(),
00099         "Rotational Velocity", 100, PixRGB<byte>(192, 255, 0));
00100 
00101   pwiiGUI->addMeter(controller->getXAccelPtr(),
00102         "X Acceleration", 255, PixRGB<byte>(192, 192, 255));
00103   pwiiGUI->addMeter(controller->getYAccelPtr(),
00104         "Y Acceleration", 255, PixRGB<byte>(192, 192, 255));
00105   pwiiGUI->addMeter(controller->getZAccelPtr(),
00106         "Z Acceleration", 255, PixRGB<byte>(192, 192, 255));
00107 
00108   pwiiGUI->addMeter(controller->getBatteryPtr(),
00109           "Wiimote Battery", 255, PixRGB<byte>(100,100,100));
00110 
00111   pwiiGUI->addImage(controller->getIRImagePtr());
00112 
00113 }
00114 
00115 /* END GUI STUFF */
00116 
00117 
00118 int getKey(nub::soft_ref<OutputFrameSeries> &ofs)
00119 {
00120   const nub::soft_ref<ImageDisplayStream> ids =
00121     ofs->findFrameDestType<ImageDisplayStream>();
00122 
00123   const rutz::shared_ptr<XWinManaged> uiwin =
00124     ids.is_valid()
00125     ? ids->getWindow("Output")
00126     : rutz::shared_ptr<XWinManaged>();
00127   return uiwin->getLastKeyPress();
00128 }
00129 
00130 
00131 int main(int argc, const char **argv)
00132 {
00133   // Instantiate a ModelManager:
00134   ModelManager *manager = new ModelManager("Test wiimote");
00135 
00136   nub::soft_ref<OutputFrameSeries> ofs(new OutputFrameSeries(*manager));
00137   manager->addSubComponent(ofs);
00138 
00139   //Create the GUI
00140   nub::soft_ref<GeneralGUI> pwiiGUI(new GeneralGUI(*manager, "PWiiGUI", "PWiiGUI", Dims(700,512)));
00141   manager->addSubComponent(pwiiGUI);
00142 
00143   //Create the PWiiBot Controller
00144   nub::soft_ref<PWiiController> controller(new PWiiController(*manager));
00145   manager->addSubComponent(controller);
00146 
00147   // Parse command-line:
00148   if (manager->parseCommandLine(argc, argv, "", 0, 0) == false) return(1);
00149 
00150 
00151 
00152   manager->exportOptions(MC_RECURSE);
00153   manager->start();
00154   sleep(1);
00155   setupGUI(pwiiGUI, ofs, controller);
00156 
00157 
00158 
00159   while(1) {
00160 
00161   }
00162 
00163 //  sleep(1);
00164 
00165 
00166 #ifdef HAVE_LIBWIIMOTE
00167 /*        wiimote_t wiimote = WIIMOTE_INIT;
00168         //wiimote_report_t report = WIIMOTE_REPORT_INIT;
00169 
00170   LINFO("Press buttons 1 and 2 on the wiimote now to connect.");
00171   int nmotes = wiimote_discover(&wiimote, 1);
00172   if (nmotes == 0)
00173     LFATAL("no wiimotes were found");
00174 
00175   LINFO("found: %s\n", wiimote.link.r_addr);
00176 
00177   if (wiimote_connect(&wiimote, wiimote.link.r_addr) < 0) {
00178     LFATAL("Unable to connect to wiimote");
00179     Raster::waitForKey();
00180     }
00181     */
00182 
00183   /* Activate the first led on the wiimote. It will take effect on the
00184      next call to wiimote_update. */
00185 /*
00186   wiimote.led.one  = 1;
00187 
00188   // let's get all our ModelComponent instances started:
00189   LINFO("Open ");
00190 
00191   LINFO("Status %i", wiimote_is_open(&wiimote));
00192 
00193   int speed = 100;
00194   int motor1_dir = 0;
00195   int motor1_vel = 0;
00196 
00197   int motor2_dir = 0;
00198   int motor2_vel = 0;
00199 
00200   Point2D<int> loc(128,128);
00201 
00202 */
00203 
00204 //  while(wiimote_is_open(&wiimote))
00205 //  {
00206     /* The wiimote_update function is used to synchronize the wiimote
00207        object with the real wiimote. It should be called as often as
00208        possible in order to minimize latency. */
00209 
00210   /*  if (wiimote_update(&wiimote) < 0) {
00211       wiimote_disconnect(&wiimote);
00212       break;
00213     }*/
00214 
00215                 /* The wiimote object has member 'keys' which keep track of the
00216                    current key state. */
00217         /*
00218                 if (wiimote.keys.home) { //press home to exit
00219                             wiimote_write_byte(&wiimote, 0x04a40001, motor1_dir);
00220                             wiimote_write_byte(&wiimote, 0x04a40002, 0);
00221                            wiimote_write_byte(&wiimote, 0x04a40003, motor2_dir);
00222                             wiimote_write_byte(&wiimote, 0x04a40004, 0);
00223 
00224                         LINFO("Shutting Down Motors and Gracefully Disconnecting...");
00225 
00226                         wiimote_disconnect(&wiimote);
00227 
00228                         LINFO("Disconnected, Goodbye!");
00229                 }
00230                 */
00231 
00232                 /* Activate the accelerometer when the 'A' key is pressed. */
00233                 //if (wiimote.keys.a) {
00234                 //        wiimote.mode.acc = 1;
00235                 //}
00236                 //else {
00237                 //        wiimote.mode.acc = 0;
00238                 //}
00239 
00240 
00241    // Image<PixRGB<byte> > img(255,255,ZEROS);
00242 
00243 //    drawLine(img, Point2D<int>(128, 128), Point2D<int>(128+(int)(wiimote.force.x*400), 128), PixRGB<byte>(255,0,0),3);
00244 //    drawLine(img, Point2D<int>(128, 128), Point2D<int>(128, 128+(int)(wiimote.force.y*400)), PixRGB<byte>(0,255,0),3);
00245 //    drawLine(img, Point2D<int>(128, 128), Point2D<int>(128+(int)(wiimote.force.z*400), 128), PixRGB<byte>(0,0,255),3);
00246 
00247    // ofs->writeRGB(img, "Output", FrameInfo("output", SRC_POS));
00248 
00249  /*
00250     int key = getKey(ofs);
00251     if (key != -1)
00252     {
00253       switch(key)
00254       {
00255         case 10:  //l
00256           speed += 10;
00257           break;
00258         case 24:
00259           speed -= 10;
00260           break;
00261         case KEY_UP:
00262           motor1_dir = 2;
00263           motor2_dir = 2;
00264           break;
00265         case KEY_DOWN:
00266           motor1_dir = 1;
00267           motor2_dir = 1;
00268           break;
00269         case KEY_LEFT:
00270           motor1_dir = 2;
00271           motor2_dir = 1;
00272           break;
00273         case KEY_RIGHT:
00274           motor1_dir = 1;
00275           motor2_dir = 2;
00276           break;
00277         case 65: //space
00278           motor1_dir = 4; motor1_vel = 0;
00279           motor2_dir = 4; motor2_vel = 0;
00280           break;
00281 
00282       }
00283         LINFO("Key: %d  -- Sending Motor Command...", key);
00284     //send the data to the wiimote
00285     wiimote_write_byte(&wiimote, 0x04a40001, motor1_dir);
00286     wiimote_write_byte(&wiimote, 0x04a40002, speed);
00287     wiimote_write_byte(&wiimote, 0x04a40003, motor2_dir);
00288     wiimote_write_byte(&wiimote, 0x04a40004, speed);
00289     }*/
00290 
00291 
00292 
00293           /*
00294                 LINFO("KEYS %04x one=%d two=%d a=%d b=%d <=%d >=%d ^=%d v=%d h=%d +=%d -=%d\n",
00295                         wiimote.keys.bits,
00296                         wiimote.keys.one,
00297                         wiimote.keys.two,
00298                         wiimote.keys.a,
00299                         wiimote.keys.b,
00300                         wiimote.keys.left,
00301                         wiimote.keys.right,
00302                         wiimote.keys.up,
00303                         wiimote.keys.down,
00304                         wiimote.keys.home,
00305                         wiimote.keys.plus,
00306                         wiimote.keys.minus);
00307 
00308                 LINFO("TILT x=%.3f y=%.3f z=%.3f\n",
00309                         wiimote.tilt.x,
00310                         wiimote.tilt.y,
00311                         wiimote.tilt.z);
00312 
00313                 LINFO("FORCE x=%.3f y=%.3f z=%.3f (sum=%.3f)\n",
00314                         wiimote.force.x,
00315                         wiimote.force.y,
00316                         wiimote.force.z,
00317       sqrt(wiimote.force.x*wiimote.force.x+wiimote.force.y*wiimote.force.y+wiimote.force.z*wiimote.force.z));*/
00318 
00319 //  }
00320 
00321   // stop all our ModelComponents
00322   manager->stop();
00323 #else
00324   LFATAL("Need the libwiimote");
00325 #endif
00326 
00327   // all done!
00328   return 0;
00329 }
00330 
00331 
00332 
00333 
00334 
00335 // ######################################################################
00336 /* So things look consistent in everyone's emacs... */
00337 /* Local Variables: */
00338 /* indent-tabs-mode: nil */
00339 /* End: */
Generated on Sun May 8 08:41:31 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3