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)
00024 commName.removePrimitiveListener(this);
00025 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00026 if(comm!=NULL)
00027 comm->close();
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;
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) {
00049 ThreadNS::Lock& l = comm->getLock();
00050 unsigned int t=get_time();
00051
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();
00064 std::ostream os(&comm->getWriteStreambuf());
00065 unsigned int curt = get_time();
00066 if(curt>=t)
00067 os << s << '\r' << flush;
00068 else {
00069 dt=t-curt;
00070 os << s << 'T' << dt << '\r' << flush;
00071 }
00072 }
00073
00074 MotionHook::motionCheck(outputs);
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;
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)
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
00103
00104
00105
00106 if((!isFirstCheck || DataSource::requiresFirstSensor) && queryServos) {
00107
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;
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;
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;
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;
00146 }
00147 ++dcnt;
00148 dq << char('A'+i) << ' ' << char('A'+i) << "L ";
00149 }
00150 }
00151
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
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
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)
00236 commName.removePrimitiveListener(this);
00237 }
00238 DataSource::setDataSourceThread(th);
00239 }
00240
00241 void SSC32Driver::plistValueChanged(const plist::PrimitiveBase& pl) {
00242 if(&pl==&commName) {
00243
00244
00245 CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00246 if(comm!=NULL) {
00247
00248 if(sensorsActive)
00249 comm->close();
00250 if(motionActive)
00251 comm->close();
00252 }
00253 comm = CommPort::getRegistry().getInstance(commName);
00254 if(comm!=NULL) {
00255
00256 if(sensorsActive)
00257 comm->open();
00258 if(motionActive)
00259 comm->open();
00260 }
00261 } else if(&pl==&queryServos) {
00262
00263
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
00277
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;
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
00292 float outRange = outputRanges[outputIdx][MaxRange]-outputRanges[outputIdx][MinRange];
00293
00294 unsigned int servoRange = maxPW[servoIdx]-minPW[servoIdx];
00295
00296 float cmd = (v-outputRanges[outputIdx][MinRange])/outRange;
00297
00298
00299 cmd=1-cmd;
00300
00301 float pw = cmd*servoRange+minPW[servoIdx];
00302
00303 unsigned int bpw = static_cast<unsigned int>(pw+0.5);
00304
00305 if(bpw<minPW[servoIdx])
00306 bpw=minPW[servoIdx];
00307 if(bpw>maxPW[servoIdx])
00308 bpw=maxPW[servoIdx];
00309
00310 ss << '#' << servoIdx << " P" << bpw << ' ';
00311 }
00312
00313 float SSC32Driver::getServo(unsigned int servoIdx, unsigned int pw) {
00314 unsigned int outputIdx = servos[servoIdx];
00315
00316 double outRange = outputRanges[outputIdx][MaxRange]-outputRanges[outputIdx][MinRange];
00317
00318 unsigned int servoRange = maxPW[servoIdx]-minPW[servoIdx];
00319
00320 return (pw-minPW[servoIdx])*outRange/servoRange + outputRanges[outputIdx][MinRange];
00321 }
00322
00323 float SSC32Driver::getAnalog(unsigned int , unsigned char s) {
00324 return s*5.f/256;
00325 }
00326
00327 float SSC32Driver::getDigital(unsigned int , unsigned char cur, unsigned char latch) {
00328 if(cur=='0')
00329 return 0;
00330 return (latch=='0') ? 0.5f : 1;
00331 }
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342