/** * Draw the layer. * * @param tileSet The tile set to use for non-flipped tiles * @param flippedTileSet The tile set to use for flipped tiles */ void JJ2Layer::draw (SDL_Surface* tileSet, SDL_Surface* flippedTileSet) { SDL_Rect src, dst; int vX, vY; int x, y; // Set tile drawing dimensions src.w = TTOI(1); src.h = TTOI(1); src.x = 0; // Calculate the layer view vX = FTOI(FTOI(viewX) * xSpeed); vY = FTOI(FTOI(viewY) * ySpeed); if (limit) { if (!tileX) { if (vX + canvasW > TTOI(width)) vX = TTOI(width) - canvasW; } if (!tileY) { vY -= canvasH - SH; if (vY + canvasH > TTOI(height)) vY = TTOI(height) - canvasH; } } for (y = 0; y <= ITOT(canvasH - 1) + 1; y++) { for (x = 0; x <= ITOT(canvasW - 1) + 1; x++) { dst.x = TTOI(x) - (vX & 31); dst.y = TTOI(y) - (vY & 31); src.y = TTOI(getTile(x + ITOT(vX), y + ITOT(vY))); if (src.y) SDL_BlitSurface(getFlipped(x + ITOT(vX), y + ITOT(vY))? flippedTileSet: tileSet, &src, canvas, &dst); } } return; }
AREXPORT bool ArUrg::blockingConnect(void) { if (!getRunning()) runAsync(); myConnMutex.lock(); if (myConn == NULL) { ArLog::log(ArLog::Terse, "%s: Could not connect because there is no connection defined", getName()); myConnMutex.unlock(); failedToConnect(); return false; } ArSerialConnection *serConn = NULL; serConn = dynamic_cast<ArSerialConnection *>(myConn); if (serConn != NULL) serConn->setBaud(atoi(getStartingBaudChoice())); if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN && !myConn->openSimple()) { ArLog::log(ArLog::Terse, "%s: Could not connect because the connection was not open and could not open it", getName()); myConnMutex.unlock(); failedToConnect(); return false; } myConnMutex.unlock(); lockDevice(); myTryingToConnect = true; unlockDevice(); laserPullUnsetParamsFromRobot(); laserCheckParams(); setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped()); ArUtil::sleep(100); bool connected = false; if (internalConnect()) connected = true; if (connected) { lockDevice(); myIsConnected = true; myTryingToConnect = false; unlockDevice(); ArLog::log(ArLog::Normal, "%s: Connected to laser", getName()); laserConnect(); return true; } else { failedToConnect(); return false; } }
bool ArUrg_2_0::internalConnect(void) { bool ret = true; char buf[1024]; ArSerialConnection *serConn = NULL; serConn = dynamic_cast<ArSerialConnection *>(myConn); bool alreadyAtAutobaud = false; // empty the buffer... /* sendCommandAndRecvStatus( "RS", "reset", buf, sizeof(buf), 1000); readLine(buf, sizeof(buf), 1, true, false); sendCommandAndRecvStatus( "SCIP2.0", "SCIP 2.0 request", buf, sizeof(buf), 1000); */ writeLine("RS"); ArUtil::sleep(100); writeLine("SCIP2.0"); ArUtil::sleep(100); ArTime startedFlushing; while (readLine(buf, sizeof(buf), 1, true, false) || startedFlushing.mSecSince() < 1000); buf[0] = '\0'; if (!(ret = sendCommandAndRecvStatus( "VV", "version request", buf, sizeof(buf), 10000)) || strcasecmp(buf, "00") != 0) { // if we didn't get it and have an autobaud, try it at what the autobaud rate is if (serConn != NULL && atoi(getAutoBaudChoice()) > 0) { alreadyAtAutobaud = true; serConn->setBaud(atoi(getAutoBaudChoice())); ArUtil::sleep(100); writeLine("RS"); ArUtil::sleep(100); writeLine("SCIP2.0"); ArUtil::sleep(100); startedFlushing.setToNow(); while (readLine(buf, sizeof(buf), 1, true, false) || startedFlushing.mSecSince() < 1000); if (!(ret = sendCommandAndRecvStatus( "VV", "version request after falling back to autobaudchoice", buf, sizeof(buf), 10000)) || strcasecmp(buf, "00") != 0) { if (ret && strcasecmp(buf, "00") != 0) ArLog::log(ArLog::Normal, "%s::blockingConnect: Bad status on version response after falling back to autobaudchoice", getName()); return false; } } // if we don't have a serial port or no autobaud then we can't // change the baud, so just fail else { if (ret && strcasecmp(buf, "00") != 0) ArLog::log(ArLog::Normal, "%s::blockingConnect: Bad status on version response (%s)", getName(), buf); return false; } } // if we want to autobaud, then give it a whirl if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0) { // empty the buffer from the last version request while (readLine(buf, sizeof(buf), 100, true, false)); // now change the baud... sprintf(buf, "SS%06d", atoi(getAutoBaudChoice())); if (!writeLine(buf)) return false; ArUtil::sleep(100); //serConn->setBaud(115200); serConn->setBaud(atoi(getAutoBaudChoice())); // wait a second for the baud to change... ArUtil::sleep(100); // empty the buffer from the baud change while (readLine(buf, sizeof(buf), 100, true, false)); if (!(ret = sendCommandAndRecvStatus( "VV", "version request after switching to autobaudchoice", buf, sizeof(buf), 10000)) || strcasecmp(buf, "00") != 0) { if (ret && strcasecmp(buf, "00") != 0) ArLog::log(ArLog::Normal, "%s::blockingConnect: Bad status on version response after switching to autobaudchoice", getName()); return false; } ArLog::log(ArLog::Verbose, "%s: Switched to %s baud rate", getName(), getAutoBaudChoice()); } while (readLine(buf, sizeof(buf), 10000, false, true)) { if (strlen(buf) == 0) break; if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0) myVendor = &buf[5]; else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0) myProduct = &buf[5]; else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0) myFirmwareVersion = &buf[5]; else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0) myProtocolVersion = &buf[5]; else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0) mySerialNumber = &buf[5]; else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0) myStat = &buf[5]; } if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() || myProtocolVersion.empty() || mySerialNumber.empty()) { ArLog::log(ArLog::Normal, "%s::blockingConnect: Missing information in version response", getName()); return false; } if (!(ret = sendCommandAndRecvStatus( "PP", "parameter info request", buf, sizeof(buf), 10000)) || strcasecmp(buf, "00") != 0) { ArLog::log(ArLog::Normal, "%s::blockingConnect: Bad response to parameter info request", getName()); return false; } while (readLine(buf, sizeof(buf), 10000, false, true)) { if (strlen(buf) == 0) break; if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0) myModel = &buf[5]; else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0) myDMin = atoi(&buf[5]); else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0) myDMax = atoi(&buf[5]); else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0) myARes = atoi(&buf[5]); else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0) myAMin = atoi(&buf[5]); else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0) myAMax = atoi(&buf[5]); else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0) myAFront = atoi(&buf[5]); else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0) myScan = atoi(&buf[5]); } if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 || myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0) { ArLog::log(ArLog::Normal, "%s::blockingConnect: Missing information in parameter info response", getName()); return false; } myStepSize = 360.0 / myARes; myStepFirst = myAFront * myStepSize; if (myMaxRange > myDMax) setMaxRange(myDMax); //log(); setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped()); //myLogMore = true; // myLogMore = false; ArUtil::sleep(100); //printf("myRequestString %s\n", myRequestString); if (!(ret = sendCommandAndRecvStatus( myRequestString, "request distance reading", buf, sizeof(buf), 10000)) || strcasecmp(buf, "00") != 0) { if (ret && strcasecmp(buf, "00") != 0) ArLog::log(ArLog::Normal, "%s::blockingConnect: Bad status on distance reading response (%s)", getName(), buf); return false; } //myLogMore = false; ArTime started; started.setToNow(); while (started.secSince() < 10 && readLine(buf, sizeof(buf), 10000, true, false)) { if (strlen(buf) == 0) return true; } ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back", getName()); return false; }
AREXPORT bool ArSick::isLaserFlipped(void) { return getFlipped(); }