PWiiController.C

00001 #ifndef PWIICONTROLLER_C_DEFINED
00002 #define PWIICONTROLLER_C_DEFINED
00003 
00004 #include "Robots/PWiiBot/PWiiController.H"
00005 #include "Component/ModelOptionDef.H"
00006 #include "Devices/DeviceOpts.H"
00007 #include "Util/Assert.H"
00008 #include "Image/DrawOps.H"
00009 #include "Util/Timer.H"
00010 
00011 enum DIRECTION {FORWARD, BACKWARD, RELEASE};
00012 
00013 
00014 
00015 namespace
00016 {
00017   class WiimoteLoop : public JobWithSemaphore
00018   {
00019   public:
00020     WiimoteLoop(PWiiController* controller)
00021       :
00022       itsController(controller),
00023       itsPriority(1),
00024       itsJobType("Wiimote Controller Loop"),
00025       itsTimer()
00026     {}
00027 
00028     virtual ~WiimoteLoop() {}
00029 
00030     virtual void run()
00031     {
00032       int breakTime, updateTime;
00033       ASSERT(itsController);
00034       while(1)
00035         {
00036          if(itsController->getConnectionStatus()) {
00037                  breakTime = itsTimer.getReset();
00038                  itsController->updateWiimote();
00039                  updateTime = itsTimer.getReset();
00040                  LINFO("Update took %dms with %dms break time  TOTAL: %dms" , updateTime, breakTime, breakTime+updateTime);
00041                  usleep(10000);
00042          }
00043 
00044         }
00045     }
00046 
00047     virtual const char* jobType() const
00048     { return itsJobType.c_str(); }
00049 
00050     virtual int priority() const
00051     { return itsPriority; }
00052 
00053   private:
00054     PWiiController* itsController;
00055     const int itsPriority;
00056     const std::string itsJobType;
00057     Timer itsTimer;
00058   };
00059 }
00060 
00061 
00062 
00063 
00064 
00065 
00066 
00067 
00068 PWiiController::PWiiController(OptionManager& mgr,
00069                              const std::string& descrName,
00070                              const std::string& tagName):
00071                              ModelComponent(mgr, descrName, tagName),
00072                              itsMotor1Speed(0), itsMotor2Speed(0),itsMotor1Dir(0), itsMotor2Dir(0),
00073                              itsTransVel(0), itsRotVel(0),
00074                              itsXAccel(0), itsYAccel(0), itsZAccel(0),
00075                              itsIR1X(0), itsIR1Y(0), itsIR1Size(0),
00076                              itsIR2X(0), itsIR2Y(0), itsIR2Size(0),
00077                              itsIR3X(0), itsIR3Y(0), itsIR3Size(0),
00078                              itsIR4X(0), itsIR4Y(0), itsIR4Size(0),
00079                              itsBatteryStatus(0), itsConnectionStatus(false),
00080                              itsIRImage(255, 255, ZEROS),
00081                              paramCurrentTransVel("Translational Velocity", this, 0, ALLOW_ONLINE_CHANGES),
00082                                paramCurrentRotVel("Rotational Velocity", this, 0, ALLOW_ONLINE_CHANGES),
00083                                paramMotor1Speed("Motor1 Speed", this, 0, ALLOW_ONLINE_CHANGES),
00084                                paramMotor2Speed("Motor2 Speed", this, 0, ALLOW_ONLINE_CHANGES),
00085                                 paramMotor1Dir("Motor1 Direction", this, 0, ALLOW_ONLINE_CHANGES),
00086                                paramMotor2Dir("Motor2 Direction", this, 0, ALLOW_ONLINE_CHANGES),
00087                                paramDrawIR("Draw IR", this, 0, ALLOW_ONLINE_CHANGES),
00088                                paramConnected("Wiimote Connection", this, 0, ALLOW_ONLINE_CHANGES) {
00089 
00090 }
00091 
00092 PWiiController::~PWiiController() {
00093 
00094                     wiimote_write_byte(&itsWiimote, 0x04a40001, 0);
00095                     wiimote_write_byte(&itsWiimote, 0x04a40002, 0);
00096                     wiimote_write_byte(&itsWiimote, 0x04a40003, 0);
00097                     wiimote_write_byte(&itsWiimote, 0x04a40004, 0);
00098 
00099 
00100                 wiimote_disconnect(&itsWiimote);
00101 }
00102 
00103 
00104 bool PWiiController::setTransVel(int transVel) {
00105         itsTransVel = transVel;
00106 
00107         if(itsTransVel > 100) {
00108                 itsTransVel = 100;
00109         }
00110         else if(itsTransVel < -100) {
00111                 itsTransVel = -100;
00112         }
00113 
00114         itsMotor1Speed = transVel + itsRotVel;
00115         itsMotor2Speed = transVel - itsRotVel;
00116         return true;
00117 
00118 }
00119 
00120 bool PWiiController::setRotVel(int rotVel) {
00121         itsRotVel = rotVel;
00122 
00123         if(itsRotVel > 100) {
00124                 itsRotVel = 100;
00125 
00126         }
00127         else if(itsRotVel < -100) {
00128                 itsRotVel = -100;
00129         }
00130 
00131         //Switch signs?
00132         itsMotor1Speed = itsTransVel + itsRotVel;
00133         itsMotor2Speed = itsTransVel - itsRotVel;
00134         return true;
00135 
00136 }
00137 
00138 bool PWiiController::setMotor1Speed(int speed) {
00139         itsMotor1Speed = speed;
00140         if(itsMotor1Speed < 0) {
00141                 itsMotor1Dir = BACKWARD;
00142                 itsMotor1Speed = -itsMotor1Speed;
00143         }
00144         else if(itsMotor1Speed > 0)
00145                 itsMotor1Dir = FORWARD;
00146         else if(itsMotor1Speed == 0)
00147                 itsMotor1Dir = RELEASE;
00148 
00149         return true;
00150 }
00151 
00152 bool PWiiController::setMotor2Speed(int speed) {
00153         itsMotor2Speed = speed;
00154 
00155         if(itsMotor2Speed < 0) {
00156                 itsMotor2Dir = BACKWARD;
00157                 itsMotor2Speed = -itsMotor2Speed;
00158         }
00159         else if(itsMotor2Speed > 0)
00160                 itsMotor2Dir = FORWARD;
00161         else if(itsMotor2Speed == 0)
00162                 itsMotor2Dir = RELEASE;
00163 
00164         return true;
00165 }
00166 
00167 bool PWiiController::setMotor1Dir(int dir) {
00168         itsMotor1Dir = dir;
00169         return true;
00170 }
00171 
00172 bool PWiiController::setMotor2Dir(int dir) {
00173         itsMotor2Dir = dir;
00174         return true;
00175 }
00176 
00177 void PWiiController::paramChanged(ModelParamBase* const param, const bool valueChanged, ParamClient::ChangeStatus* status) {
00178         if (param == &paramMotor1Speed && valueChanged){
00179                 setMotor1Speed(paramMotor1Speed.getVal());
00180         }
00181         else if (param == &paramMotor2Speed && valueChanged){
00182                 setMotor2Speed(paramMotor2Speed.getVal());
00183         }
00184         else if (param == &paramMotor1Dir && valueChanged){
00185                 setMotor1Dir(paramMotor1Dir.getVal());
00186         }
00187         else if (param == &paramMotor2Dir && valueChanged){
00188                 setMotor2Dir(paramMotor2Dir.getVal());
00189         }
00190         else if (param == &paramCurrentTransVel && valueChanged){
00191                 setTransVel(paramCurrentTransVel.getVal());
00192         }
00193         else if (param == &paramCurrentRotVel && valueChanged){
00194                 setRotVel(paramCurrentRotVel.getVal());
00195         }
00196         else if(param == &paramDrawIR && valueChanged){
00197 
00198                 if(paramDrawIR.getVal() == false) {
00199                   itsIRImage.clear(PixRGB<byte>(0,0,0));
00200                 }
00201         }
00202         else if(param == &paramConnected && valueChanged) {
00203         //TODO: Fix bugs here... If the wiimote fails to connect, we need to set the paramConnected to false
00204                 if(paramConnected.getVal() == true) {
00205                         if(!connectWiimote()) {
00206                                 itsConnectionStatus = false;
00207                         }
00208                 }
00209                 if(paramConnected.getVal() == false) {
00210                         itsConnectionStatus = false;
00211                 }
00212         }
00213 
00214 
00215 }
00216 
00217 
00218 void PWiiController::start2()
00219 {
00220 
00221   //setup wiimote update thread
00222   itsThreadServer.reset(new WorkThreadServer("PWiiController",1)); //start a single worker thread
00223   itsThreadServer->setFlushBeforeStopping(false);
00224   rutz::shared_ptr<WiimoteLoop> j(new WiimoteLoop(this));
00225   itsThreadServer->enqueueJob(j);
00226 
00227 }
00228 
00229 bool PWiiController::connectWiimote() {
00230         //itsWiimote = WIIMOTE_INIT;
00231 
00232 
00233 
00234         LINFO("Press buttons 1 and 2 on the wiimote now to connect.");
00235         int nmotes = wiimote_discover(&itsWiimote, 1);
00236 
00237   if (nmotes == 0)  {
00238     LINFO("no wiimotes were found");
00239     itsConnectionStatus = false;
00240     return false;
00241     }
00242 
00243   LINFO("found: %s\n", itsWiimote.link.r_addr);
00244 
00245   if (wiimote_connect(&itsWiimote, itsWiimote.link.r_addr) < 0) {
00246      LINFO("Unable to connect to wiimote");
00247      itsConnectionStatus = false;
00248      return false;
00249     }
00250 
00251 
00252     //Enable acceleration and IR data from the wiimote as well as nunchuck data
00253     itsWiimote.mode.acc = 1;
00254     itsWiimote.mode.ir  = 1;
00255     itsWiimote.mode.ext = 1;
00256 
00257   LINFO("Status %i", wiimote_is_open(&itsWiimote));
00258 
00259   itsConnectionStatus = true;
00260   return true;
00261 }
00262 
00263 bool PWiiController::updateWiimote() {
00264 
00265 
00266         if(!itsConnectionStatus) {
00267                 LINFO("Connection Status is False");
00268                 paramConnected.setVal(false);
00269                 return false;
00270         }
00271 
00272         if(!wiimote_is_open(&itsWiimote)) {
00273                 LINFO("Could not communicate with wiimote");
00274                 itsConnectionStatus = false;
00275                 paramConnected.setVal(false);
00276                 return false;
00277         }
00278 
00279 
00280 
00281         /*wiimote_write_byte(&itsWiimote, 0x04a40001, itsMotor1Dir);
00282             wiimote_write_byte(&itsWiimote, 0x04a40002, itsMotor1Speed);
00283             wiimote_write_byte(&itsWiimote, 0x04a40003, itsMotor2Dir);
00284            wiimote_write_byte(&itsWiimote, 0x04a40004, itsMotor2Speed);*/
00285            uint8_t buffer[4] = {itsMotor1Dir, itsMotor1Speed, itsMotor2Dir, itsMotor2Speed};
00286            wiimote_write(&itsWiimote, 0x04a40001, buffer, 4);
00287            usleep(10000);
00288 
00289         if (wiimote_update(&itsWiimote) < 0) {
00290                 LINFO("Could not update wiimote");
00291                 itsConnectionStatus = false;
00292                 paramConnected.setVal(false);
00293                 return false;
00294         }
00295 
00296         itsBatteryStatus = itsWiimote.battery;
00297 
00298         itsXAccel = itsWiimote.axis.x;
00299         itsYAccel = itsWiimote.axis.y;
00300         itsZAccel = itsWiimote.axis.z;
00301 
00302         itsIR1X = itsWiimote.ir1.x;
00303         itsIR1Y = itsWiimote.ir1.y;
00304         itsIR1Size = itsWiimote.ir1.size;
00305 
00306         itsIR2X = itsWiimote.ir2.x;
00307         itsIR2Y = itsWiimote.ir2.y;
00308         itsIR2Size = itsWiimote.ir2.size;
00309 
00310         itsIR3X = itsWiimote.ir3.x;
00311         itsIR3Y = itsWiimote.ir3.y;
00312         itsIR3Size = itsWiimote.ir3.size;
00313 
00314         itsIR4X = itsWiimote.ir4.x;
00315         itsIR4Y = itsWiimote.ir4.y;
00316         itsIR4Size = itsWiimote.ir4.size;
00317 
00318 
00319 
00320 
00321 
00322 
00323         if(paramDrawIR.getVal())
00324                 updateIRImage();
00325 
00326         return true;
00327 }
00328 
00329 //Redraw the IR Image
00330 void PWiiController::updateIRImage() {
00331         itsIRImage.clear(PixRGB<byte>(20,20,20));
00332 
00333         Point2D<int> IRPoint;
00334 
00335 
00336         if(itsIR1Size < 15) {
00337 
00338                 IRPoint = Point2D<int>(float(itsIR1X)/1023.0*itsIRImage.getWidth(),float(itsIR1Y)/767.0*itsIRImage.getHeight());
00339 
00340                 drawDisk(itsIRImage, IRPoint, itsIR1Size*2, PixRGB<byte>(255,255,255));
00341 
00342                 writeText(itsIRImage, IRPoint, "1", PixRGB<byte>(255, 0, 0), PixRGB<byte>(0,0,0), SimpleFont::FIXED(8), true);
00343 
00344         }
00345 
00346         if(itsIR2Size < 15) {
00347                 IRPoint = Point2D<int>(float(itsIR2X)/1023.0*itsIRImage.getWidth(),float(itsIR2Y)/767.0*itsIRImage.getHeight());
00348 
00349                 drawDisk(itsIRImage,
00350                 IRPoint, itsIR2Size*2, PixRGB<byte>(255,255,255));
00351 
00352                 writeText(itsIRImage, IRPoint, "2", PixRGB<byte>(255, 0, 0), PixRGB<byte>(0,0,0), SimpleFont::FIXED(8), true);
00353         }
00354 
00355         if(itsIR3Size < 15) {
00356                 IRPoint = Point2D<int>(float(itsIR3X)/1023.0*itsIRImage.getWidth(),float(itsIR3Y)/767.0*itsIRImage.getHeight());
00357 
00358                 drawDisk(itsIRImage,
00359                 IRPoint, itsIR3Size*2, PixRGB<byte>(255,255,255));
00360 
00361                 writeText(itsIRImage, IRPoint, "3", PixRGB<byte>(255, 0, 0), PixRGB<byte>(0,0,0), SimpleFont::FIXED(8), true);
00362         }
00363 
00364         if(itsIR4Size < 15) {
00365                 IRPoint = Point2D<int>(float(itsIR4X)/1023.0*itsIRImage.getWidth(),float(itsIR4Y)/767.0*itsIRImage.getHeight());
00366                 drawDisk(itsIRImage,
00367                 IRPoint, itsIR4Size*2, PixRGB<byte>(255,255,255));
00368 
00369                 writeText(itsIRImage, IRPoint, "4", PixRGB<byte>(255, 0, 0), PixRGB<byte>(0,0,0), SimpleFont::FIXED(8), true);
00370         }
00371 
00372 
00373 
00374 
00375 }
00376 #endif
00377 
00378 
00379 
Generated on Sun May 8 08:41:31 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3