00001 #include <arpa/inet.h>
00002 #include <stdio.h>
00003 #include "CreateCommands.h"
00004 #include "CreateDriver.h"
00005 #include "Shared/RobotInfo.h"
00006 #include "Shared/MarkScope.h"
00007 #include "Shared/get_time.h"
00008 #include "Shared/debuget.h"
00009
00010 using namespace std;
00011
00012 const std::string CreateDriver::autoRegisterCreateDriver = DeviceDriver::getRegistry().registerType<CreateDriver>("Create");
00013
00014 void CreateDriver::motionStarting() {
00015 std::cout << "motionStarting called!" << std::endl;
00016 MotionHook::motionStarting();
00017 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00018 if(comm!=NULL){
00019 comm->open();
00020
00021 std::cout << "Motion Starting Connect" << std::endl;
00022
00023 connect();
00024 }
00025 motionActive=true;
00026 commName.addPrimitiveListener(this);
00027 }
00028
00029 void CreateDriver::motionStopping() {
00030 motionActive=false;
00031 if(!sensorsActive)
00032 commName.removePrimitiveListener(this);
00033 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00034 if(comm!=NULL)
00035 comm->close();
00036 MotionHook::motionStopping();
00037 }
00038
00039 void CreateDriver::motionCheck(const float outputs[][NumOutputs]) {
00040 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00041 if(comm==NULL || !comm->isWriteable())
00042 return;
00043
00044 stringstream ss;
00045
00046 unsigned short output;
00047
00048 ss << CREATE_DRIVE_DIRECT;
00049
00050
00051 output = (unsigned short)(outputs[NumFrames-1][1] * 10);
00052
00053 ss << (char)(output >> 8);
00054 ss << (char)(output & 0xFF);
00055
00056
00057 output = (unsigned short)(outputs[NumFrames-1][0] * 10);
00058
00059 ss << (char)(output >> 8);
00060 ss << (char)(output & 0xFF);
00061
00062
00063
00064 string s=ss.str();
00065 if(s.size()>0) {
00066 ThreadNS::Lock& l = comm->getLock();
00067 unsigned int t=get_time();
00068
00069 unsigned int dt = static_cast<unsigned int>(NumFrames*FrameTime/((getTimeScale()>0)?getTimeScale():1.f));
00070 unsigned int giveup = t+dt*3/4;
00071 t+=dt;
00072 while(!l.trylock()) {
00073 if(get_time()>=giveup) {
00074
00075
00076
00077
00078 return;
00079 }
00080 usleep(1000);
00081 }
00082 MarkScope autolock(l); l.unlock();
00083 std::ostream os(&comm->getWriteStreambuf());
00084 dt=t-get_time();
00085 os << s << flush;
00086 }
00087
00088 MotionHook::motionCheck(outputs);
00089 }
00090
00091
00092
00093 void CreateDriver::connect(){
00094 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00095 ThreadNS::Lock& l = comm->getLock();
00096 unsigned int t=get_time();
00097 unsigned int giveup = t+3000000;
00098 while(!l.trylock()) {
00099 if(get_time()>=giveup) {
00100 if(MotionHook::verbose>0)
00101 cerr << "Unable to connect: couldn't get lock on comm port" << endl;
00102 return;
00103 }
00104 usleep(1000);
00105 }
00106 MarkScope autolock(l); l.unlock();
00107
00108 std::cout << "SENDING START/SAFE COMMANDS" << std::endl;
00109 std::ostream os(&comm->getWriteStreambuf());
00110 os << CREATE_START << std::flush;
00111 os << CREATE_SAFE << std::flush;
00112
00113
00114 os << CREATE_STREAM << (char)1 << CREATE_SENSOR_GROUP_6 << std::flush;
00115 }
00116
00117 unsigned int CreateDriver::nextTimestamp() {
00118 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00119 if(comm==NULL || !comm->isReadable())
00120 return -1U;
00121 return get_time();
00122 }
00123
00124 unsigned char CreateDriver::readChar(std::istream &is){
00125 return (unsigned char) is.get();
00126 }
00127
00128 short CreateDriver::readShort(std::istream &is){
00129 return ((short) is.get() << 8) | ((short) is.get());
00130 }
00131
00132 int CreateDriver::readPacket(std::istream &is, const char &type){
00133 int total = 0;
00134 switch(type){
00135 case CREATE_SENSOR_GROUP_0:
00136 total += readPacket(is, CREATE_SENSOR_DROP);
00137 total += readPacket(is, CREATE_SENSOR_WALL);
00138 total += readPacket(is, CREATE_SENSOR_CLIFF_LEFT);
00139 total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_LEFT);
00140 total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_RIGHT);
00141 total += readPacket(is, CREATE_SENSOR_CLIFF_RIGHT);
00142 total += readPacket(is, CREATE_SENSOR_VIRTUAL_WALL);
00143 total += readPacket(is, CREATE_SENSOR_OVERCURRENT);
00144 total += readPacket(is, CREATE_SENSOR_UNUSED_1);
00145 total += readPacket(is, CREATE_SENSOR_UNUSED_2);
00146 total += readPacket(is, CREATE_SENSOR_IR);
00147 total += readPacket(is, CREATE_SENSOR_BUTTONS);
00148 total += readPacket(is, CREATE_SENSOR_DISTANCE);
00149 total += readPacket(is, CREATE_SENSOR_ANGLE);
00150 total += readPacket(is, CREATE_SENSOR_CHANGING_STATE);
00151 total += readPacket(is, CREATE_SENSOR_VOLTAGE);
00152 total += readPacket(is, CREATE_SENSOR_CURRENT);
00153 total += readPacket(is, CREATE_SENSOR_BATTERY_TEMP);
00154 total += readPacket(is, CREATE_SENSOR_BATTERY_CHARGE);
00155 total += readPacket(is, CREATE_SENSOR_BATTERY_CAPACITY);
00156 break;
00157 case CREATE_SENSOR_GROUP_1:
00158 total += readPacket(is, CREATE_SENSOR_DROP);
00159 total += readPacket(is, CREATE_SENSOR_WALL);
00160 total += readPacket(is, CREATE_SENSOR_CLIFF_LEFT);
00161 total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_LEFT);
00162 total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_RIGHT);
00163 total += readPacket(is, CREATE_SENSOR_CLIFF_RIGHT);
00164 total += readPacket(is, CREATE_SENSOR_VIRTUAL_WALL);
00165 total += readPacket(is, CREATE_SENSOR_OVERCURRENT);
00166 total += readPacket(is, CREATE_SENSOR_UNUSED_1);
00167 total += readPacket(is, CREATE_SENSOR_UNUSED_2);
00168 break;
00169 case CREATE_SENSOR_GROUP_2:
00170 total += readPacket(is, CREATE_SENSOR_IR);
00171 total += readPacket(is, CREATE_SENSOR_BUTTONS);
00172 total += readPacket(is, CREATE_SENSOR_DISTANCE);
00173 total += readPacket(is, CREATE_SENSOR_ANGLE);
00174 break;
00175 case CREATE_SENSOR_GROUP_3:
00176 total += readPacket(is, CREATE_SENSOR_CHANGING_STATE);
00177 total += readPacket(is, CREATE_SENSOR_VOLTAGE);
00178 total += readPacket(is, CREATE_SENSOR_CURRENT);
00179 total += readPacket(is, CREATE_SENSOR_BATTERY_TEMP);
00180 total += readPacket(is, CREATE_SENSOR_BATTERY_CHARGE);
00181 total += readPacket(is, CREATE_SENSOR_BATTERY_CAPACITY);
00182 break;
00183 case CREATE_SENSOR_GROUP_4:
00184 total += readPacket(is, CREATE_SENSOR_WALL_SIGNAL);
00185 total += readPacket(is, CREATE_SENSOR_CLIFF_L_SIGNAL);
00186 total += readPacket(is, CREATE_SENSOR_CLIFF_FL_SIGNAL);
00187 total += readPacket(is, CREATE_SENSOR_CLIFF_FR_SIGNAL);
00188 total += readPacket(is, CREATE_SENSOR_CLIFF_R_SIGNAL);
00189 total += readPacket(is, CREATE_SENSOR_DIGITAL_IN);
00190 total += readPacket(is, CREATE_SENSOR_ANALOG);
00191 total += readPacket(is, CREATE_SENSOR_CAN_CHARGE);
00192 break;
00193 case CREATE_SENSOR_GROUP_5:
00194 total += readPacket(is, CREATE_SENSOR_OI_MODE);
00195 total += readPacket(is, CREATE_SENSOR_SONG_NUMBER);
00196 total += readPacket(is, CREATE_SENSOR_SONG_PLAYING);
00197 total += readPacket(is, CREATE_SENSOR_STREAM_SIZE);
00198 total += readPacket(is, CREATE_SENSOR_REQ_VELOCITY);
00199 total += readPacket(is, CREATE_SENSOR_REQ_RADIUS);
00200 total += readPacket(is, CREATE_SENSOR_REQ_RIGHT_VELOCITY);
00201 total += readPacket(is, CREATE_SENSOR_REQ_LEFT_VELOCITY);
00202 break;
00203 case CREATE_SENSOR_GROUP_6:
00204 total += readPacket(is, CREATE_SENSOR_DROP);
00205 total += readPacket(is, CREATE_SENSOR_WALL);
00206 total += readPacket(is, CREATE_SENSOR_CLIFF_LEFT);
00207 total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_LEFT);
00208 total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_RIGHT);
00209 total += readPacket(is, CREATE_SENSOR_CLIFF_RIGHT);
00210 total += readPacket(is, CREATE_SENSOR_VIRTUAL_WALL);
00211 total += readPacket(is, CREATE_SENSOR_OVERCURRENT);
00212 total += readPacket(is, CREATE_SENSOR_UNUSED_1);
00213 total += readPacket(is, CREATE_SENSOR_UNUSED_2);
00214 total += readPacket(is, CREATE_SENSOR_IR);
00215 total += readPacket(is, CREATE_SENSOR_BUTTONS);
00216 total += readPacket(is, CREATE_SENSOR_DISTANCE);
00217 total += readPacket(is, CREATE_SENSOR_ANGLE);
00218 total += readPacket(is, CREATE_SENSOR_CHANGING_STATE);
00219 total += readPacket(is, CREATE_SENSOR_VOLTAGE);
00220 total += readPacket(is, CREATE_SENSOR_CURRENT);
00221 total += readPacket(is, CREATE_SENSOR_BATTERY_TEMP);
00222 total += readPacket(is, CREATE_SENSOR_BATTERY_CHARGE);
00223 total += readPacket(is, CREATE_SENSOR_BATTERY_CAPACITY);
00224 total += readPacket(is, CREATE_SENSOR_WALL_SIGNAL);
00225 total += readPacket(is, CREATE_SENSOR_CLIFF_L_SIGNAL);
00226 total += readPacket(is, CREATE_SENSOR_CLIFF_FL_SIGNAL);
00227 total += readPacket(is, CREATE_SENSOR_CLIFF_FR_SIGNAL);
00228 total += readPacket(is, CREATE_SENSOR_CLIFF_R_SIGNAL);
00229 total += readPacket(is, CREATE_SENSOR_DIGITAL_IN);
00230 total += readPacket(is, CREATE_SENSOR_ANALOG);
00231 total += readPacket(is, CREATE_SENSOR_CAN_CHARGE);
00232 total += readPacket(is, CREATE_SENSOR_OI_MODE);
00233 total += readPacket(is, CREATE_SENSOR_SONG_NUMBER);
00234 total += readPacket(is, CREATE_SENSOR_SONG_PLAYING);
00235 total += readPacket(is, CREATE_SENSOR_STREAM_SIZE);
00236 total += readPacket(is, CREATE_SENSOR_REQ_VELOCITY);
00237 total += readPacket(is, CREATE_SENSOR_REQ_RADIUS);
00238 total += readPacket(is, CREATE_SENSOR_REQ_RIGHT_VELOCITY);
00239 total += readPacket(is, CREATE_SENSOR_REQ_LEFT_VELOCITY);
00240 break;
00241 case CREATE_SENSOR_STREAM_SIZE:
00242 total = 1;
00243 readChar(is);
00244 break;
00245 case CREATE_SENSOR_DROP:
00246 total = 1;
00247 createStatus.bumpsWheelDrops = readChar(is);
00248 break;
00249 case CREATE_SENSOR_WALL:
00250 total = 1;
00251 createStatus.wall = readChar(is);
00252 break;
00253 case CREATE_SENSOR_CLIFF_LEFT:
00254 total = 1;
00255 createStatus.cliffLeft = readChar(is);
00256 break;
00257 case CREATE_SENSOR_CLIFF_FRONT_LEFT:
00258 total = 1;
00259 createStatus.cliffFrontLeft = readChar(is);
00260 break;
00261 case CREATE_SENSOR_CLIFF_FRONT_RIGHT:
00262 total = 1;
00263 createStatus.cliffFrontRight = readChar(is);
00264 break;
00265 case CREATE_SENSOR_CLIFF_RIGHT:
00266 total = 1;
00267 createStatus.cliffRight = readChar(is);
00268 break;
00269 case CREATE_SENSOR_VIRTUAL_WALL:
00270 total = 1;
00271 createStatus.virtualWall = readChar(is);
00272 break;
00273 case CREATE_SENSOR_OVERCURRENT:
00274 total = 1;
00275 createStatus.overcurrents = readChar(is);
00276 break;
00277 case CREATE_SENSOR_IR:
00278 total = 1;
00279 createStatus.ir = readChar(is);
00280 break;
00281 case CREATE_SENSOR_BUTTONS:
00282 total = 1;
00283 createStatus.buttons = readChar(is);
00284 break;
00285 case CREATE_SENSOR_DISTANCE:
00286 total = 2;
00287 createStatus.distance = readShort(is);
00288 break;
00289 case CREATE_SENSOR_ANGLE:
00290 total = 2;
00291 createStatus.angle = readShort(is);
00292 break;
00293 case CREATE_SENSOR_CHANGING_STATE:
00294 total = 1;
00295 createStatus.chargingState = readChar(is);
00296 break;
00297 case CREATE_SENSOR_VOLTAGE:
00298 total = 2;
00299 createStatus.voltage = readShort(is);
00300 break;
00301 case CREATE_SENSOR_CURRENT:
00302 total = 2;
00303 createStatus.current = readShort(is);
00304 break;
00305 case CREATE_SENSOR_BATTERY_TEMP:
00306 total = 1;
00307 createStatus.batteryTemperature = readChar(is);
00308 break;
00309 case CREATE_SENSOR_BATTERY_CHARGE:
00310 total = 2;
00311 createStatus.batteryCharge = readShort(is);
00312 break;
00313 case CREATE_SENSOR_BATTERY_CAPACITY:
00314 total = 2;
00315 createStatus.batteryCapacity = readShort(is);
00316 break;
00317 case CREATE_SENSOR_WALL_SIGNAL:
00318 total = 2;
00319 createStatus.wallSignal = readShort(is);
00320 break;
00321 case CREATE_SENSOR_CLIFF_L_SIGNAL:
00322 total = 2;
00323 createStatus.cliffLeftSignal = readShort(is);
00324 break;
00325 case CREATE_SENSOR_CLIFF_FL_SIGNAL:
00326 total = 2;
00327 createStatus.cliffFrontLeftSignal = readShort(is);
00328 break;
00329 case CREATE_SENSOR_CLIFF_FR_SIGNAL:
00330 total = 2;
00331 createStatus.cliffFrontRightSignal = readShort(is);
00332 break;
00333 case CREATE_SENSOR_CLIFF_R_SIGNAL:
00334 total = 2;
00335 createStatus.cliffRightSignal = readShort(is);
00336 break;
00337 case CREATE_SENSOR_DIGITAL_IN:
00338 total = 1;
00339 createStatus.userDigitalInputs= readChar(is);
00340 break;
00341 case CREATE_SENSOR_ANALOG:
00342 total = 2;
00343 createStatus.userAnalogInput = readShort(is);
00344 break;
00345 case CREATE_SENSOR_CAN_CHARGE:
00346 total = 1;
00347 createStatus.chargingSourcesAvailable = readChar(is);
00348 break;
00349 case CREATE_SENSOR_OI_MODE:
00350 total = 1;
00351 createStatus.oiMode = readChar(is);
00352 break;
00353 case CREATE_SENSOR_SONG_NUMBER:
00354 total = 1;
00355 createStatus.songNumber = readChar(is);
00356 break;
00357 case CREATE_SENSOR_SONG_PLAYING:
00358 total = 1;
00359 createStatus.songPlay = readChar(is);
00360 break;
00361 case CREATE_SENSOR_REQ_VELOCITY:
00362 total = 2;
00363 createStatus.velocity = readShort(is);
00364 break;
00365 case CREATE_SENSOR_REQ_RADIUS:
00366 total = 2;
00367 createStatus.radius = readShort(is);
00368 break;
00369 case CREATE_SENSOR_REQ_RIGHT_VELOCITY:
00370 total = 2;
00371 createStatus.rightVelocity = readShort(is);
00372 break;
00373 case CREATE_SENSOR_REQ_LEFT_VELOCITY:
00374 total = 2;
00375 createStatus.leftVelocity = readShort(is);
00376 break;
00377 case CREATE_SENSOR_UNUSED_1:
00378 case CREATE_SENSOR_UNUSED_2:
00379 total = 1;
00380 readChar(is);
00381 break;
00382 default:
00383 std::cerr << "CREATE DRIVER: unknown packet type " << (int) type << std::endl;
00384 break;
00385 }
00386
00387 return total;
00388 }
00389
00390 unsigned int CreateDriver::getData(const char *& payload, unsigned int& payloadSize, unsigned int& timestamp, std::string& name) {
00391 std::cout << "getData call!" << std::endl;
00392 payload=NULL; payloadSize=0;
00393 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00394 if(comm==NULL || !comm->isReadable() || !comm->isWriteable())
00395 return frameNumber;
00396 unsigned int t=get_time();
00397 if(timestamp>t)
00398 usleep(static_cast<unsigned int>((timestamp-t)*1000/(getTimeScale()>0?getTimeScale():1.f)));
00399 if(thread==NULL)
00400 return frameNumber;
00401 stringstream ss;
00402 timestamp=get_time();
00403 ss << "#POS\n";
00404 ss << "condensed " << RobotInfo::RobotName << "\n";
00405 ss << "meta-info = ";
00406 ss << t;
00407 ss << " ";
00408 ss << t;
00409 ss << "\n";
00410 std::cout << "begin read" << std::endl;
00411 {
00412 MarkScope autolock(comm->getLock());
00413 std::ostream os(&comm->getWriteStreambuf());
00414 std::istream is(&comm->getReadStreambuf());
00415
00416 os << CREATE_STREAM << (char)1 << CREATE_SENSOR_GROUP_6 << std::flush;
00417
00418
00419 unsigned char in=0;
00420 unsigned char type=0;
00421 unsigned char numPackets = 0;
00422 int i;
00423
00424
00425 while(1){
00426 type = readChar(is);
00427 if(type == 19){
00428 numPackets = readChar(is);
00429 printf("reading %d packets\n", numPackets);
00430 for(i = 0; i < numPackets; i+=in){
00431 type = readChar(is);
00432 printf("reading type %d\n", type);
00433 in = readPacket(is, type) + 1;
00434 printf("read %d packets\n", in);
00435 }
00436 if(i != numPackets){
00437 std::cerr << "CREATE DRIVER: read in more than expected! Read " << i << " Expected " << (int)numPackets << std::endl;
00438 }
00439 printf("CREATE DRIVER: checksum %d\n", readChar(is));
00440 break;
00441 }
00442 else{
00443 std::cerr << "CREATE DRIVER: did not get packet with data '19' as expected! Got: " << (unsigned short) type << std::endl;
00444 }
00445 }
00446
00447 }
00448 ss << "outputs =" << createStatus.leftVelocity << " " << createStatus.rightVelocity << " 0 0" << "\n";
00449 ss << "sensors =";
00450 ss << (createStatus.userDigitalInputs & 0x1) << " ";
00451 ss << ((createStatus.userDigitalInputs >> 1) & 0x1) << " ";
00452 ss << ((createStatus.userDigitalInputs >> 2) & 0x1) << " ";
00453 ss << ((createStatus.userDigitalInputs >> 3) & 0x1) << " ";
00454 ss << createStatus.userAnalogInput << " ";
00455 ss << createStatus.wallSignal << " ";
00456 ss << createStatus.ir << " ";
00457 ss << createStatus.cliffLeftSignal << " ";
00458 ss << createStatus.cliffFrontLeftSignal << " ";
00459 ss << createStatus.cliffFrontRightSignal << " ";
00460 ss << createStatus.cliffRightSignal << " ";
00461 ss << createStatus.distance << " ";
00462 ss << createStatus.angle << " ";
00463 ss << "0 0 0 0 0\n";
00464 ss << "#END\n";
00465 lastSensor=ss.str();
00466
00467 payload=lastSensor.c_str();
00468 payloadSize=lastSensor.size();
00469 name=nextName();
00470 std::cout << "finish getData!" << std::endl;
00471 return frameNumber++;
00472 }
00473
00474 void CreateDriver::setDataSourceThread(LoadDataThread* th) {
00475 std::cout << "setDataSounceThread called!!" << std::endl;
00476 if(thread==NULL && th!=NULL) {
00477 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00478 if(comm!=NULL){
00479 comm->open();
00480
00481 std::cout << "set data sounce thread" << std::endl;
00482
00483 connect();
00484 }
00485 sensorsActive=true;
00486 commName.addPrimitiveListener(this);
00487 }
00488 if(thread!=NULL && th==NULL) {
00489 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00490 if(comm!=NULL)
00491 comm->close();
00492 sensorsActive=false;
00493 if(!motionActive)
00494 commName.removePrimitiveListener(this);
00495
00496 }
00497 DataSource::setDataSourceThread(th);
00498 }
00499
00500 void CreateDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00501 std::cout << "plistvalueChanged Called!" << std::endl;
00502 if(&pl==&commName) {
00503
00504
00505 CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00506 if(comm!=NULL) {
00507
00508 if(sensorsActive)
00509 comm->close();
00510 if(motionActive)
00511 comm->close();
00512 }
00513 comm = CommPort::getRegistry().getInstance(commName);
00514 if(comm!=NULL) {
00515
00516 if(sensorsActive)
00517 comm->open();
00518 if(motionActive)
00519 comm->open();
00520
00521
00522 connect();
00523 }
00524 }
00525 }
00526
00527 float CreateDriver::getAnalog(unsigned int , unsigned char s) {
00528 return s*5.f/256;
00529 }
00530
00531 float CreateDriver::getDigital(unsigned int , unsigned char cur, unsigned char latch) {
00532 if(cur=='0')
00533 return 0;
00534 return (latch=='0') ? 0.5f : 1;
00535 }
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546