Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

SSC32Driver.cc

Go to the documentation of this file.
00001 #include "SSC32Driver.h"
00002 #include "Shared/MarkScope.h"
00003 #include "Shared/get_time.h"
00004 #include "Shared/debuget.h"
00005 
00006 using namespace std; 
00007 
00008 const unsigned int SSC32Driver::NUM_SERVO;
00009 const int SSC32Driver::UNUSED;
00010 const std::string SSC32Driver::autoRegisterSSC32Driver = DeviceDriver::getRegistry().registerType<SSC32Driver>("SSC32");
00011 
00012 void SSC32Driver::motionStarting() {
00013   MotionHook::motionStarting();
00014   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00015   if(comm!=NULL)
00016     comm->open();
00017   motionActive=true;
00018   commName.addPrimitiveListener(this);
00019 }
00020 
00021 void SSC32Driver::motionStopping() {
00022   motionActive=false;
00023   if(!sensorsActive) // listener count is not recursive, so only remove if we're the last one
00024     commName.removePrimitiveListener(this);
00025   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00026   if(comm!=NULL)
00027     comm->close(); // this *is* recursive, so we always close it to match our open() in motionStarting()
00028   MotionHook::motionStopping();
00029 }
00030 
00031 void SSC32Driver::motionCheck(const float outputs[][NumOutputs]) {
00032   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00033   if(comm==NULL || !comm->isWriteable())
00034     return;
00035   
00036   stringstream ss;
00037   for(unsigned int i=0; i<NUM_SERVO; i++) {
00038     int idx=servos[i];
00039     if(idx<0 || static_cast<unsigned int>(idx)>=NumOutputs) {
00040       if(idx!=UNUSED)
00041         std::cerr << "Warning: SSC32 driver mapping servo " << i << " to invalid output index " << idx << std::endl;
00042       continue; // invalid/unused servo
00043     }
00044     if(isFirstCheck || !sparse || lastOutputs[idx]!=outputs[NumFrames-1][idx])
00045       setServo(ss, i, outputs[NumFrames-1][idx]);
00046   }
00047   string s=ss.str();
00048   if(s.size()>0) { // if sparse and no changes, skip update altogether
00049     ThreadNS::Lock& l = comm->getLock();
00050     unsigned int t=get_time();
00051     // keep trying to get the lock, sleeping 1 ms each time, until 3/4 the frame time is gone (then give up)
00052     unsigned int dt = static_cast<unsigned int>(NumFrames*FrameTime/((getTimeScale()>0)?getTimeScale():1.f));
00053     unsigned int giveup = t+dt*3/4;
00054     t+=dt;
00055     while(!l.trylock()) {
00056       if(get_time()>=giveup) {
00057         if(MotionHook::verbose>0)
00058           cerr << "Dropping SSC32 motion update: couldn't get lock on comm port" << endl;
00059         return;
00060       }
00061       usleep(1000);
00062     }
00063     MarkScope autolock(l); l.unlock(); //transfer lock to MarkScope
00064     std::ostream os(&comm->getWriteStreambuf());
00065     unsigned int curt = get_time();
00066     if(curt>=t) // too late!
00067       os << s << '\r' << flush;
00068     else {
00069       dt=t-curt;
00070       os << s << 'T' << dt << '\r' << flush; // indicate time until next update
00071     }
00072   }
00073   
00074   MotionHook::motionCheck(outputs); // updates lastOutputs and isFirstCheck, we ignore its motionUpdated() call
00075 }
00076 
00077 
00078 unsigned int SSC32Driver::nextTimestamp() {
00079   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00080   if(comm==NULL || !comm->isReadable())
00081     return -1U;
00082   return get_time();
00083 }
00084 
00085 unsigned int SSC32Driver::getData(const char *& payload, unsigned int& payloadSize, unsigned int& timestamp, std::string& name) {
00086   payload=NULL; payloadSize=0; // in case of error
00087   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00088   if(comm==NULL || !comm->isReadable() || !comm->isWriteable())
00089     return frameNumber;
00090   unsigned int t=get_time();
00091   if(timestamp>t)
00092     usleep(static_cast<unsigned int>((timestamp-t)*1000/(getTimeScale()>0?getTimeScale():1.f)));
00093   if(thread==NULL) // in case we shut down while waiting
00094     return frameNumber;
00095   stringstream ss;
00096   timestamp=get_time();
00097   ss << "#POS\n";
00098   {
00099     MarkScope autolock(comm->getLock());
00100     std::ostream os(&comm->getWriteStreambuf());
00101     std::istream is(&comm->getReadStreambuf());
00102     // generally, bad idea to request servo position before position has been sent
00103     // But if the arm is still powered, it'll know it's position.  If the user has set WaitForSensors,
00104     // then DataSource::requiresFirstSensor will be set, otherwise we skip queries until we've sent
00105     // a servo position (and thus set the isFirstCheck flag
00106     if((!isFirstCheck || DataSource::requiresFirstSensor) && queryServos) {
00107       // check joint positions
00108       stringstream q;
00109       for(unsigned int i=0; i<NUM_SERVO; i++) {
00110         int idx=servos[i];
00111         if(idx<0 || static_cast<unsigned int>(idx)>=NumOutputs)
00112           continue; // invalid/unused servo
00113         q << "QP " << i << ' ';
00114       }
00115       os << q.rdbuf() << '\r' << flush;
00116       for(unsigned int i=0; i<NUM_SERVO; i++) {
00117         int idx=servos[i];
00118         if(idx<0 || static_cast<unsigned int>(idx)>=NumOutputs)
00119           continue; // invalid/unused servo
00120         int check=is.get();
00121         if(check==-1) {
00122           cerr << "SSC32Driver: bad read!" << endl;
00123           return frameNumber;
00124         }
00125         unsigned int v=(unsigned char)check;
00126         ss << outputNames[idx] << '\t' << getServo(i,v*10) << "\t1\n";
00127       }
00128     }
00129     stringstream aq,dq;
00130     bool acnt=0,dcnt=0;
00131     for(unsigned int i=0; i<NUM_INPUT; i++) {
00132       int idx=inputs[i];
00133       if(!buttonMode[i]) {
00134         if(idx<0 || static_cast<unsigned int>(idx)>=NumSensors) {
00135           if(idx!=UNUSED)
00136             std::cerr << "Warning: SSC32 driver mapping input " << i << " to invalid sensor index " << idx << std::endl;
00137           continue; // invalid/unused servo
00138         }
00139         ++acnt;
00140         aq << 'V' << char('A'+i) << ' ';
00141       } else {
00142         if(idx<0 || static_cast<unsigned int>(idx)>=NumButtons) {
00143           if(idx!=UNUSED)
00144             std::cerr << "Warning: SSC32 driver mapping input " << i << " to invalid button index " << idx << std::endl;
00145           continue; // invalid/unused servo
00146         }
00147         ++dcnt;
00148         dq << char('A'+i) << ' ' << char('A'+i) << "L ";
00149       }
00150     }
00151     // send both queries now, we can process first response while SSC is processing second query
00152     if(dcnt>0)
00153       os << dq.rdbuf() << '\r';
00154     if(acnt>0)
00155       os << aq.rdbuf() << '\r';
00156     if(dcnt>0 || acnt>0)
00157       os << flush;
00158     // store responses
00159     if(dcnt>0) {
00160       ss << "<buttons>\n";
00161       for(unsigned int i=0; i<NUM_INPUT; i++) {
00162         int idx=inputs[i];
00163         if(idx>=0 && static_cast<unsigned int>(idx)<NumButtons && buttonMode[i]) {
00164           int check=is.get();
00165           if(check==-1) {
00166             cerr << "SSC32Driver: bad read!" << endl;
00167             return frameNumber;
00168           }
00169           unsigned char cur=check;
00170           check=is.get();
00171           if(check==-1) {
00172             cerr << "SSC32Driver: bad read!" << endl;
00173             return frameNumber;
00174           }
00175           unsigned char latch=check;
00176           float v=getDigital(i,cur,latch);
00177           ss << buttonNames[idx] << '\t' << v << '\n';
00178         }
00179       }
00180       ss << "</buttons>\n";
00181     }
00182     if(acnt>0) {
00183       ss << "<sensors>\n";
00184       for(unsigned int i=0; i<NUM_INPUT; i++) {
00185         int idx=inputs[i];
00186         if(idx>=0 && static_cast<unsigned int>(idx)<NumSensors && !buttonMode[i]) {
00187           int check=is.get();
00188           if(check==-1) {
00189             cerr << "SSC32Driver: bad read!" << endl;
00190             return frameNumber;
00191           }
00192           float v=getAnalog(i,check);
00193           ss << sensorNames[idx] << '\t' << v << '\n';
00194         }
00195       }
00196       ss << "</sensors>\n";
00197     }
00198   }
00199   ss << "#END\n";
00200   lastSensor=ss.str();
00201   //cerr << lastSensor << endl;
00202   payload=lastSensor.c_str();
00203   payloadSize=lastSensor.size();
00204   name=nextName();
00205   return frameNumber++;
00206 }
00207 
00208 void SSC32Driver::setDataSourceThread(LoadDataThread* th) {
00209   if(thread==NULL && th!=NULL) {
00210     CommPort * comm = CommPort::getRegistry().getInstance(commName);
00211     if(comm!=NULL)
00212       comm->open();
00213     queryServos.addPrimitiveListener(this);
00214     if(queryServos) {
00215       for(unsigned int i=0; i<NUM_SERVO; i++) {
00216         provideOutput(servos[i]);
00217         servos[i].addPrimitiveListener(this);
00218       }
00219     }
00220     sensorsActive=true;
00221     commName.addPrimitiveListener(this);
00222   }
00223   if(thread!=NULL && th==NULL) {
00224     CommPort * comm = CommPort::getRegistry().getInstance(commName);
00225     if(comm!=NULL)
00226       comm->close();
00227     if(queryServos) {
00228       for(unsigned int i=0; i<NUM_SERVO; ++i) {
00229         servos[i].removePrimitiveListener(this);
00230         ignoreOutput(servos[i]);
00231       }
00232     }
00233     queryServos.removePrimitiveListener(this);
00234     sensorsActive=false;
00235     if(!motionActive) // listener count is not recursive, so only remove if we're the last one
00236       commName.removePrimitiveListener(this);
00237   }
00238   DataSource::setDataSourceThread(th);
00239 }
00240 
00241 void SSC32Driver::plistValueChanged(const plist::PrimitiveBase& pl) {
00242   if(&pl==&commName) {
00243     // if here, then motionStarted or setDataSourceThread has been called, thus when commName changes,
00244     // need to close old one and reopen new one
00245     CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00246     if(comm!=NULL) {
00247       // close each of our old references
00248       if(sensorsActive)
00249         comm->close();
00250       if(motionActive)
00251         comm->close();
00252     }
00253     comm = CommPort::getRegistry().getInstance(commName);
00254     if(comm!=NULL) {
00255       // open each of our new references
00256       if(sensorsActive)
00257         comm->open();
00258       if(motionActive)
00259         comm->open();
00260     }
00261   } else if(&pl==&queryServos) {
00262     // if here, LoadDataThread has been assigned, need to update providing/ignoring outputs
00263     // (and maintain listeners for individual servos while providing)
00264     if(queryServos) {
00265       for(unsigned int i=0; i<NUM_SERVO; i++) {
00266         provideOutput(servos[i]);
00267         servos[i].addPrimitiveListener(this);
00268       }
00269     } else {
00270       for(unsigned int i=0; i<NUM_SERVO; ++i) {
00271         servos[i].removePrimitiveListener(this);
00272         ignoreOutput(servos[i]);
00273       }
00274     }
00275   } else {
00276     // check if it's one of the individual servos... if it is, means we're providing servo feedback,
00277     // need to call providingOutput/ignoringOutput as appropriate
00278     for(unsigned int i=0; i<NUM_SERVO; ++i) {
00279       if(&pl==&servos[i]) {
00280         ignoreOutput(servos[i].getPreviousValue());
00281         provideOutput(servos[i]);
00282         return; // found it, DON'T fall through to error message below...
00283       }
00284     }
00285     std::cerr << "Unhandled value change in " << getClassName() << ": " << pl.get() << std::endl;
00286   }
00287 }
00288 
00289 void SSC32Driver::setServo(std::ostream& ss, unsigned int servoIdx, float v) {
00290   unsigned int outputIdx = servos[servoIdx];
00291   // get output's range in radians
00292   float outRange = outputRanges[outputIdx][MaxRange]-outputRanges[outputIdx][MinRange];
00293   // get servo's range in pulse width (ms)
00294   unsigned int servoRange = maxPW[servoIdx]-minPW[servoIdx];
00295   // get commanded position as percent of range of motion
00296   float cmd = (v-outputRanges[outputIdx][MinRange])/outRange;
00297   // flip commanded position -- map positive (high) rotation to low pulse width
00298   // this is so if you mount a servo "up", coordinate system will work correctly
00299   cmd=1-cmd;
00300   // do conversion from radians (output domain) to pulse width (servo domain)
00301   float pw = cmd*servoRange+minPW[servoIdx];
00302   // round to int
00303   unsigned int bpw = static_cast<unsigned int>(pw+0.5);
00304   // check bounds
00305   if(bpw<minPW[servoIdx])
00306     bpw=minPW[servoIdx];
00307   if(bpw>maxPW[servoIdx])
00308     bpw=maxPW[servoIdx];
00309   // send to output buffer
00310   ss << '#' << servoIdx << " P" << bpw << ' ';
00311 }
00312 
00313 float SSC32Driver::getServo(unsigned int servoIdx, unsigned int pw) {
00314   unsigned int outputIdx = servos[servoIdx];
00315   // get output's range in radians
00316   double outRange = outputRanges[outputIdx][MaxRange]-outputRanges[outputIdx][MinRange];
00317   // get servo's range in pulse width (ms)
00318   unsigned int servoRange = maxPW[servoIdx]-minPW[servoIdx];
00319   // do conversion from pulse width (servo domain) to radians (output domain)
00320   return (pw-minPW[servoIdx])*outRange/servoRange + outputRanges[outputIdx][MinRange];
00321 }
00322 
00323 float SSC32Driver::getAnalog(unsigned int /*inputIdx*/, unsigned char s) {
00324   return s*5.f/256;
00325 }
00326 
00327 float SSC32Driver::getDigital(unsigned int /*inputIdx*/, unsigned char cur, unsigned char latch) {
00328   if(cur=='0')
00329     return 0;
00330   return (latch=='0') ? 0.5f : 1;
00331 }
00332 
00333 /*! @file
00334  * @brief 
00335  * @author Ethan Tira-Thompson (ejt) (Creator)
00336  *
00337  * $Author: ejt $
00338  * $Name: tekkotsu-4_0 $
00339  * $Revision: 1.6 $
00340  * $State: Exp $
00341  * $Date: 2007/11/01 20:25:05 $
00342  */

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4