// TODO: This function may need to be re-coded because it is too specific for // the code we're on. It might be best to have something like this in the AI part. // It might be best to just have openClaws and closeClaws. void Robot::dropBlock() { unsigned int sensors[8]; bool atPoint = false; bool delivered = false; int ticks_moved=0; turnTo(SOUTH); movePoints(1); moveTicks(60, 25); stopMoving(25); openClaws(); delay(300); moveTicks(-50, 25); stopMoving(25, true); delay(300); turnTo(NORTH); delay(300); movePoints(1); delay(500); /*while (!delivered) { ticks_moved = 0; while ((ticks_moved < 70) && !atPoint) { //Go Forward moveTicks(2, 25); ticks_moved = ticks_moved + wheelEnc.getCountsM1(); gridSensors.readCalibrated(sensors, QTR_EMITTERS_ON); if ((sensors[0] < SENSORBLACK) && (sensors[7] < SENSORBLACK) && (ticks_moved >10)) { motorStop(); delay(2000); atPoint = true; servo(SERVO_RIGHT,30); //Check required values. servo(SERVO_LEFT,80); moveTicks(-ticks_moved, 20); delivered = true; } } motorStop(); delay(2000); atPoint = false; moveTicks(-ticks_moved, 20); }*/ }
void Foam::displacementSBRStressFvMotionSolver::solve() { // The points have moved so before interpolation update // the fvMotionSolver accordingly movePoints(fvMesh_.points()); diffusivityPtr_->correct(); pointDisplacement_.boundaryField().updateCoeffs(); surfaceScalarField Df = diffusivityPtr_->operator()(); volTensorField gradCd = fvc::grad(cellDisplacement_); Foam::solve ( fvm::laplacian ( 2*Df, cellDisplacement_, "laplacian(diffusivity,cellDisplacement)" ) + fvc::div ( Df *( ( cellDisplacement_.mesh().Sf() & fvc::interpolate(gradCd.T() - gradCd) ) // Solid-body rotation "lambda" term - cellDisplacement_.mesh().Sf()*fvc::interpolate(tr(gradCd)) ) ) /* - fvc::laplacian ( 2*Df, cellDisplacement_, "laplacian(diffusivity,cellDisplacement)" ) + fvc::div ( Df *( ( cellDisplacement_.mesh().Sf() & fvc::interpolate(gradCd + gradCd.T()) ) // Solid-body rotation "lambda" term - cellDisplacement_.mesh().Sf()*fvc::interpolate(tr(gradCd)) ) ) */ ); }
void OpticalFlow::calculate(Mat prev, Mat img, Rect lastRect, bool clear) { if (clear) { data.boundingBox.width = 0; data.boundingBox.height = 0; data.points->clear(); } if (!data.points[0].empty() && !data.blocked) { vector<uchar> status; vector<float> err; calcOpticalFlowPyrLK(prev, img, data.points[0], data.points[1], status, err, data.winSize, 5, data.termcrit, 0, 0.001); resetNotFoundPoints(status, lastRect); movePoints(lastRect.width, lastRect.height); calculateRegion(); } }
bool Foam::movingBodyTopoFvMesh::update() { // Store points to recreate mesh motion pointField oldPointsNew = allPoints(); pointField newPoints = allPoints(); autoPtr<mapPolyMesh> topoChangeMap = topoChanger_.changeMesh(); bool localMeshChanged = topoChangeMap->morphing(); bool globalMeshChanged = localMeshChanged; reduce(globalMeshChanged, orOp<bool>()); if (globalMeshChanged) { Pout<< "Topology change. Calculating motion point mask" << endl; motionMask_ = calcMotionMask(); } if (localMeshChanged) { // // Map old points onto the new mesh // pointField mappedOldPointsNew(allPoints().size()); // mappedOldPointsNew.map(oldPointsNew, topoChangeMap->pointMap()); // movePoints(mappedOldPointsNew); // resetMotion(); // setV0(); // Get new points from preMotion newPoints = topoChangeMap().preMotionPoints(); } // else // { // // No change, use old points // movePoints(oldPointsNew); // resetMotion(); // setV0(); // } // Calculate new points using a velocity transformation newPoints += motionMask_* transform(SBMFPtr_().velocity(), newPoints)*time().deltaT().value(); Info << "Executing mesh motion" << endl; movePoints(newPoints); return localMeshChanged; }
void Foam::velocityLaplacianFvMotionSolver::solve() { // The points have moved so before interpolation update // the fvMotionSolver accordingly movePoints(fvMesh_.points()); diffusivityPtr_->correct(); pointMotionU_.boundaryFieldRef().updateCoeffs(); Foam::solve ( fvm::laplacian ( diffusivityPtr_->operator()(), cellMotionU_, "laplacian(diffusivity,cellMotionU)" ) ); }
void Foam::displacementComponentLaplacianFvMotionSolver::solve() { // The points have moved so before interpolation update // the motionSolver accordingly movePoints(fvMesh_.getPoints()); diffusivityPtr_->correct(); pointDisplacement_.boundaryField().updateCoeffs(); Foam::solve ( fvm::laplacian ( diffusivityPtr_->operator()(), cellDisplacement_, "laplacian(diffusivity,cellDisplacement)" ) ); }
bool Foam::RBFMotionFunctionObject::execute() { const polyMesh& mesh = time_.lookupObject<polyMesh>(regionName_); // Grab RBF motion solver RBFMotionSolver& ms = const_cast<RBFMotionSolver&> ( mesh.lookupObject<RBFMotionSolver>("dynamicMeshDict") ); # include "kinematicModel.H" ms.setMotion(motion); movePoints(ms.newPoints()); return true; }
void Foam::displacementMeshMoverMotionSolver::solve() { // The points have moved so before calculation update // the mesh and motionSolver accordingly movePoints(mesh().points()); // Update any point motion bcs (e.g. timevarying) pointDisplacement().boundaryFieldRef().updateCoeffs(); label nAllowableErrors = 0; labelList checkFaces(identity(mesh().nFaces())); meshMover().move ( coeffDict().optionalSubDict(meshMover().type() + "Coeffs"), nAllowableErrors, checkFaces ); // This will have updated the mesh and implicitly the pointDisplacement pointDisplacement().correctBoundaryConditions(); }
bool Foam::rawTopoChangerFvMesh::update() { // Do mesh changes (use inflation - put new points in topoChangeMap) Info<< "rawTopoChangerFvMesh : Checking for topology changes..." << endl; // Mesh not moved/changed yet moving(false); topoChanging(false); // Do any topology changes. Sets topoChanging (through polyTopoChange) autoPtr<mapPolyMesh> topoChangeMap = topoChanger_.changeMesh(true); bool hasChanged = topoChangeMap.valid(); if (hasChanged) { Info<< "rawTopoChangerFvMesh : Done topology changes..." << endl; // Temporary: fix fields on patch faces created out of nothing // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Two situations: // - internal faces inflated out of nothing // - patch faces created out of previously internal faces // Is face mapped in any way? PackedBoolList mappedFace(nFaces()); const label nOldInternal = topoChangeMap().oldPatchStarts()[0]; const labelList& faceMap = topoChangeMap().faceMap(); for (label facei = 0; facei < nInternalFaces(); facei++) { if (faceMap[facei] >= 0) { mappedFace[facei] = 1; } } for (label facei = nInternalFaces(); facei < nFaces(); facei++) { if (faceMap[facei] >= 0 && faceMap[facei] >= nOldInternal) { mappedFace[facei] = 1; } } const List<objectMap>& fromFaces = topoChangeMap().facesFromFacesMap(); forAll(fromFaces, i) { mappedFace[fromFaces[i].index()] = 1; } const List<objectMap>& fromEdges = topoChangeMap().facesFromEdgesMap(); forAll(fromEdges, i) { mappedFace[fromEdges[i].index()] = 1; } const List<objectMap>& fromPts = topoChangeMap().facesFromPointsMap(); forAll(fromPts, i) { mappedFace[fromPts[i].index()] = 1; } // Set unmapped faces to zero Info<< "rawTopoChangerFvMesh : zeroing unmapped boundary values." << endl; zeroUnmappedValues<scalar, fvPatchField, volMesh>(mappedFace); zeroUnmappedValues<vector, fvPatchField, volMesh>(mappedFace); zeroUnmappedValues<sphericalTensor, fvPatchField, volMesh>(mappedFace); zeroUnmappedValues<symmTensor, fvPatchField, volMesh>(mappedFace); zeroUnmappedValues<tensor, fvPatchField, volMesh>(mappedFace); // Special handling for phi: set unmapped faces to recreated phi Info<< "rawTopoChangerFvMesh :" << " recreating phi for unmapped boundary values." << endl; const volVectorField& U = lookupObject<volVectorField>("U"); surfaceScalarField& phi = const_cast<surfaceScalarField&> ( lookupObject<surfaceScalarField>("phi") ); setUnmappedValues ( phi, mappedFace, (linearInterpolate(U) & Sf())() ); if (topoChangeMap().hasMotionPoints()) { pointField newPoints = topoChangeMap().preMotionPoints(); // Give the meshModifiers opportunity to modify points Info<< "rawTopoChangerFvMesh :" << " calling modifyMotionPoints." << endl; topoChanger_.modifyMotionPoints(newPoints); // Actually move points Info<< "rawTopoChangerFvMesh :" << " calling movePoints." << endl; movePoints(newPoints); } } else { //Pout<< "rawTopoChangerFvMesh :" // << " no topology changes..." << endl; } return hasChanged;
bool Foam::twoStrokeEngine::update() { // Detaching the interface if (attached()) { Info << "Decoupling sliding interfaces" << endl; makeSlidersLive(); // Changing topology by hand autoPtr<mapPolyMesh> topoChangeMap5 = topoChanger_.changeMesh(); if (topoChangeMap5->hasMotionPoints() && topoChangeMap5->morphing()) { Info << "Topology change; executing pre-motion after " << "sliding detach" << endl; movePoints(topoChangeMap5->preMotionPoints()); } Info << "sliding interfaces successfully decoupled!!!" << endl; } else { Info << "Sliding interfaces decoupled" << endl; } Info << "Executing layer action" << endl; // Piston Layering makeLayersLive(); // Changing topology by hand // /* Tommaso, 23/5/2008 // Find piston mesh modifier const label pistonLayerID = topoChanger_.findModifierID("pistonLayer"); if (pistonLayerID < 0) { FatalErrorIn("void engineFvMesh::moveAndMorph()") << "Piston modifier not found." << abort(FatalError); } scalar minLayerThickness = piston().minLayer(); scalar deltaZ = engTime().pistonDisplacement().value(); virtualPistonPosition() += deltaZ; Info << "virtualPistonPosition = " << virtualPistonPosition() << ", deckHeight = " << deckHeight() << ", pistonPosition = " << pistonPosition() << endl; if (realDeformation()) { // Dectivate piston layer Info << "Mesh deformation mode" << endl; topoChanger_[pistonLayerID].disable(); } else { // Activate piston layer Info << "Piston layering mode" << endl; topoChanger_[pistonLayerID].enable(); } // Changing topology by hand autoPtr<mapPolyMesh> topoChangeMap = topoChanger_.changeMesh(); // Work array for new points position. pointField newPoints = allPoints(); const pointField& refPoints = allPoints(); if (topoChangeMap->morphing()) { if (topoChangeMap->hasMotionPoints()) { Info<< "Topology change; executing pre-motion after " << "dynamic layering" << endl; movePoints(topoChangeMap->preMotionPoints()); newPoints = topoChangeMap->preMotionPoints(); } setV0(); resetMotion(); } // Reset the position of layered interfaces boolList scaleDisp(nPoints(), true); boolList pistonPoint(newPoints.size(), false); boolList headPoint(newPoints.size(), false); // Make a list of piston and head points. HJ, 2/Dec/2009 labelList pistonPoints; labelList headPoints; { label pistonCellIndex = cellZones().findZoneID("pistonCells"); if (pistonCellIndex < 0) { FatalErrorIn("bool twoStrokeEngine::update()") << "Cannot find cell zone pistonCells" << abort(FatalError); } const labelList& pistonCells = cellZones()[pistonCellIndex]; label headCellIndex = cellZones().findZoneID("headCells"); if (headCellIndex < 0) { FatalErrorIn("bool twoStrokeEngine::update()") << "Cannot find cell zone headCells" << abort(FatalError); } const labelList& headCells = cellZones()[headCellIndex]; const labelListList& cp = cellPoints(); boolList count(newPoints.size(), false); forAll (pistonCells, cellI) { const labelList& curCellPoints = cp[pistonCells[cellI]]; forAll (curCellPoints, i) { count[curCellPoints[i]] = true; } } // Count the points label nCounted = 0; forAll (count, pointI) { if (count[pointI] == true) { nCounted++; } } pistonPoints.setSize(nCounted); // Collect the points nCounted = 0; forAll (count, pointI) { if (count[pointI] == true) { pistonPoints[nCounted] = pointI; nCounted++; } } // Repeat for head points count = false; forAll (headCells, cellI) { const labelList& curCellPoints = cp[pistonCells[cellI]]; forAll (curCellPoints, i) { count[curCellPoints[i]] = true; } } // Count the points nCounted = 0; forAll (count, pointI) { if (count[pointI] == true) { nCounted++; } } headPoints.setSize(nCounted); // Collect the points nCounted = 0; forAll (count, pointI) { if (count[pointI] == true) { headPoints[nCounted] = pointI; nCounted++; } } } label nScaled = nPoints(); // label pistonPtsIndex = pointZones().findZoneID("pistonPoints"); // const labelList& pistonPoints = pointZones()[pistonPtsIndex]; // label headPtsIndex = pointZones().findZoneID("headPoints"); // const labelList& headPoints = pointZones()[headPtsIndex]; const scalarField& movingPointsM = movingPointsMask(); forAll(pistonPoints, i) { label pointI = pistonPoints[i]; pistonPoint[pointI] = true; point& p = newPoints[pointI]; if (p.z() < pistonPosition() - 1.0e-6) { scaleDisp[pointI] = false; nScaled--; } }
void Foam::layerSmooth::addZonesAndModifiers() { // Add the zones and mesh modifiers to operate piston motion if ( /* pointZones().size() > 0 || faceZones().size() > 0 || cellZones().size() > 0 || topoChanger_.size() > 0 */ pointZones().size() > 0 && faceZones().size() > 0 && topoChanger_.size() > 0 ) { Info<< "Time = " << engTime().theta() << endl; Info<< "void Foam::layerSmooth::addZonesAndModifiers() : " << "Zones and modifiers already present. Skipping." << endl; setVirtualPositions(); checkAndCalculate(); Info << "Point zones found = " << pointZones().size() << endl; Info << "Face zones found = " << faceZones().size() << endl; Info << "Cell zones found = " << cellZones().size() << endl; return; } else { pointZones().setSize(0); faceZones().setSize(0); cellZones().setSize(0); topoChanger_.setSize(0); } if ( engTime().engineDict().found("zOffsetGambit") && engTime().engineDict().found("zDisplGambit") ) { Info << "Assembling the cylinder mesh" << endl; scalar zOffset ( readScalar(engTime().engineDict().lookup("zOffsetGambit")) ); scalar zDispl ( readScalar(engTime().engineDict().lookup("zDisplGambit")) ); const pointField& ap = allPoints(); pointField pDispl = allPoints(); forAll(ap, pointI) { const point p = ap[pointI]; if (p.z() >= zOffset) { pDispl[pointI].z() -= zDispl; } } movePoints(pDispl); write(); resetMotion(); Info << "Cylinder mesh assembled" << endl; }
void fvMotionSolverEngineMesh::move() { scalar deltaZ = engineDB_.pistonDisplacement().value(); Info<< "deltaZ = " << deltaZ << endl; // Position of the top of the static mesh layers above the piston scalar pistonPlusLayers = pistonPosition_.value() + pistonLayers_.value(); motionSolver_.cellMotionU().boundaryField()[pistonIndex_] == deltaZ; { scalarField linerPoints = motionSolver_.cellMotionU() .boundaryField()[linerIndex_].patch().Cf().component(vector::Z); motionSolver_.cellMotionU().boundaryField()[linerIndex_] == deltaZ*pos(deckHeight_.value() - linerPoints) *(deckHeight_.value() - linerPoints) /(deckHeight_.value() - pistonPlusLayers); } motionSolver_.solve(); if (engineDB_.foundObject<surfaceScalarField>("phi")) { surfaceScalarField& phi = const_cast<surfaceScalarField&> (engineDB_.lookupObject<surfaceScalarField>("phi")); const volScalarField& rho = engineDB_.lookupObject<volScalarField>("rho"); const volVectorField& U = engineDB_.lookupObject<volVectorField>("U"); bool absolutePhi = false; if (moving()) { phi += fvc::interpolate(rho)*fvc::meshPhi(rho, U); absolutePhi = true; } movePoints(motionSolver_.curPoints()); if (absolutePhi) { phi -= fvc::interpolate(rho)*fvc::meshPhi(rho, U); } } else { movePoints(motionSolver_.curPoints()); } pistonPosition_.value() += deltaZ; scalar pistonSpeed = deltaZ/engineDB_.deltaT().value(); Info<< "clearance: " << deckHeight_.value() - pistonPosition_.value() << nl << "Piston speed = " << pistonSpeed << " m/s" << endl; }
bool Foam::movingConeTopoFvMesh::update() { // Do mesh changes (use inflation - put new points in topoChangeMap) autoPtr<mapPolyMesh> topoChangeMap = topoChanger_.changeMesh(true); // Calculate the new point positions depending on whether the // topological change has happened or not pointField newPoints; vector curMotionVel_ = motionVelAmplitude_* Foam::sin(time().value()*M_PI/motionVelPeriod_); Pout<< "time:" << time().value() << " curMotionVel_:" << curMotionVel_ << " curLeft:" << curLeft_ << " curRight:" << curRight_ << endl; if (topoChangeMap.valid()) { Info<< "Topology change. Calculating motion points" << endl; if (topoChangeMap().hasMotionPoints()) { Info<< "Topology change. Has premotion points" << endl; //Info<< "preMotionPoints:" << topoChangeMap().preMotionPoints() // << endl; //mkDir(time().timePath()); //{ // OFstream str(time().timePath()/"meshPoints.obj"); // Pout<< "Writing mesh with meshPoints to " << str.name() // << endl; // // const pointField& currentPoints = points(); // label vertI = 0; // forAll(currentPoints, pointI) // { // meshTools::writeOBJ(str, currentPoints[pointI]); // vertI++; // } // forAll(edges(), edgeI) // { // const edge& e = edges()[edgeI]; // str << "l " << e[0]+1 << ' ' << e[1]+1 << nl; // } //} //{ // OFstream str(time().timePath()/"preMotionPoints.obj"); // Pout<< "Writing mesh with preMotionPoints to " << str.name() // << endl; // // const pointField& newPoints = // topoChangeMap().preMotionPoints(); // label vertI = 0; // forAll(newPoints, pointI) // { // meshTools::writeOBJ(str, newPoints[pointI]); // vertI++; // } // forAll(edges(), edgeI) // { // const edge& e = edges()[edgeI]; // str << "l " << e[0]+1 << ' ' << e[1]+1 << nl; // } //} motionMask_ = vertexMarkup ( topoChangeMap().preMotionPoints(), curLeft_, curRight_ ); // Move points inside the motionMask newPoints = topoChangeMap().preMotionPoints() + ( pos(0.5 - mag(motionMask_)) // cells above the body )*curMotionVel_*time().deltaT().value(); } else { Info<< "Topology change. Already set mesh points" << endl; motionMask_ = vertexMarkup ( points(), curLeft_, curRight_ ); // Move points inside the motionMask newPoints = points() + ( pos(0.5 - mag(motionMask_)) // cells above the body )*curMotionVel_*time().deltaT().value(); } } else { Info<< "No topology change" << endl; // Set the mesh motion newPoints = points() + ( pos(0.5 - mag(motionMask_)) // cells above the body )*curMotionVel_*time().deltaT().value(); } // The mesh now contains the cells with zero volume Info << "Executing mesh motion" << endl; movePoints(newPoints); // The mesh now has got non-zero volume cells curLeft_ = average ( faceZones() [ faceZones().findZoneID("leftExtrusionFaces") ]().localPoints() ).x() - SMALL; curRight_ = average ( faceZones() [ faceZones().findZoneID("rightExtrusionFaces") ]().localPoints() ).x() + SMALL; return true; }
void tetDecompositionEngineMesh::move() { scalar deltaZ = engineDB_.pistonDisplacement().value(); Info<< "deltaZ = " << deltaZ << endl; // Position of the top of the static mesh layers above the piston scalar pistonPlusLayers = pistonPosition_.value() + pistonLayers_.value(); pointField newPoints = points(); tetPolyMesh tetMesh(*this); // Select the set of boundary condition types. For symmetry planes // and wedge boundaries, the slip condition should be used; // otherwise, use the fixedValue wordList boundaryTypes ( boundary().size(), fixedValueTetPolyPatchScalarField::typeName ); forAll (boundary(), patchI) { if ( isType<symmetryFvPatch>(boundary()[patchI]) || isType<wedgeFvPatch>(boundary()[patchI]) || isType<emptyFvPatch>(boundary()[patchI]) ) { boundaryTypes[patchI] = slipTetPolyPatchScalarField::typeName; } } tetPointScalarField motionUz ( IOobject ( "motionUz", engineDB_.timeName(), engineDB_, IOobject::NO_READ, IOobject::NO_WRITE ), tetMesh, dimensionedScalar("0", dimLength, 0), boundaryTypes ); motionUz.boundaryField()[pistonIndex_] == deltaZ; { scalarField linerPoints = motionUz.boundaryField()[linerIndex_].patch() .localPoints().component(vector::Z); motionUz.boundaryField()[linerIndex_] == deltaZ*pos(deckHeight_.value() - linerPoints) *(deckHeight_.value() - linerPoints) /(deckHeight_.value() - pistonPlusLayers); } elementScalarField diffusion ( IOobject ( "motionDiffusion", engineDB_.timeName(), engineDB_, IOobject::NO_READ, IOobject::NO_WRITE ), tetMesh, dimensionedScalar("d", dimless, 1.0) ); const fvPatchList& patches = boundary(); forAll (patches, patchI) { const unallocLabelList& fc = patches[patchI].faceCells(); forAll (fc, fcI) { diffusion[fc[fcI]] = 2; } } solve(tetFem::laplacian(diffusion, motionUz)); newPoints.replace ( vector::Z, newPoints.component(vector::Z) + scalarField::subField ( motionUz.internalField(), newPoints.size() ) ); if (engineDB_.foundObject<surfaceScalarField>("phi")) { surfaceScalarField& phi = const_cast<surfaceScalarField&> (engineDB_.lookupObject<surfaceScalarField>("phi")); const volScalarField& rho = engineDB_.lookupObject<volScalarField>("rho"); const volVectorField& U = engineDB_.lookupObject<volVectorField>("U"); bool absolutePhi = false; if (moving()) { phi += fvc::interpolate(rho)*fvc::meshPhi(rho, U); absolutePhi = true; } movePoints(newPoints); if (absolutePhi) { phi -= fvc::interpolate(rho)*fvc::meshPhi(rho, U); } } pistonPosition_.value() += deltaZ; scalar pistonSpeed = deltaZ/engineDB_.deltaT().value(); Info<< "clearance: " << deckHeight_.value() - pistonPosition_.value() << nl << "Piston speed = " << pistonSpeed << " m/s" << endl; }