Пример #1
0
void BiclopsAccess::recordLimits()
{
	for(int j = 0; j < BICLOPS_NUM_JOINTS; j++)
	{
		long hlf = 0, hlr = 0, slf = 0, slr = 0;  
		PMDAxisControl *axis = _biclops->GetAxis(j);
	    axis->GetHomingLimits(hlf, hlr, slf, slr);

		limits[j][0] = axis->CountsToUnits(hlr);
		limits[j][1] = axis->CountsToUnits(hlf);
	}
}
Пример #2
0
double BiclopsAccess::getJointDesiredPosition(int jnt)
{
	// Can only ask during HOMING, POSITION and PARKED. 
	if(_mode <= HOMING) return 0.0;

	PMDint32 d; 
	PMDAxisControl *axis = _biclops->GetAxis(jnt);

	axis->GetCommandedPosition(d);
	double p = axis->CountsToUnits(d);
	p = unitsToRadians(p, jnt);

	return p;
}
Пример #3
0
double BiclopsAccess::getJointPosition(int jnt)
{
	// Can only ask during HOMING, POSITION and PARKED. 
	if(_mode <= HOMING) return 0.0;

	PMDint32 d; 
	PMDAxisControl *axis = _biclops->GetAxis(jnt);

	axis->GetActualPosition(d);
	
	double p = axis->CountsToUnits(d);

/*
	if (jnt == 0) {
	  std::cout<<"Inside joint position1 :"<<(int) d<< " --> " << p << " cpUnit: " << axis->GetCountsPerEncoderCycle() << std::endl;
	}
*/
	p = unitsToRadians(p, jnt);	

	return p;
}
Пример #4
0
// ------------------------------------------------------------------------
bool PMDNetwork::ChangeCommSettings(int baud) {

    tagPMDSerialBaudRate    pmdBaud;
    tagPMDSerialParity      pmdParity;
    tagPMDSerialStopBits    pmdStopBits;
    tagPMDSerialProtocol    pmdProtocol;
    PMDuint8                pmdMultiDropID;

    // Command the new comm settings to each controller.
    ControllerCollection::iterator iControl;
    for(iControl=controllers->begin();iControl!=controllers->end();iControl++) {
        PMDController *controller = (*iControl).second;
        PMDAxisControl *axis = controller->GetPrimaryAxis();
        axis->GetSerialPortMode(pmdBaud,pmdParity,
            pmdStopBits,pmdProtocol,pmdMultiDropID);
        axis->SetSerialPortMode(PMDSerial::HostToPMDBaud(baud),pmdParity,
            pmdStopBits,pmdProtocol,pmdMultiDropID);
    }

    // Change the network's common serial port to the new commm settings.
    // Change the host interface settings to match.
    char portName[32];
    int oldBaud;
    PortableSerial::Parity parity;
    PortableSerial:: StopBits stopBits;
    comm->GetConfig(portName,oldBaud,parity,stopBits);  // Get port settings
    PMDSerial::ConstrainSerialSettings(baud,parity,stopBits);
    comm->SetConfig(portName,baud,parity,stopBits); // Change port settings
    comm->Open();   // Reopens port with new settings

    // Verify connectivity to each controller at the new settings.
    for(iControl=controllers->begin();iControl!=controllers->end();iControl++) {
        PMDController *controller = (*iControl).second;
//        PMDAxisControl *axis = controller->GetPrimaryAxis();
        controller->GetPrimaryAxis(); // Prime the axis' primary axis storage
        if (((PMDSerial *)controller->GetIOTransport())->Sync() != PMD_ERR_OK)
            return false;
    }
    return true;
}
Пример #5
0
void BiclopsAccess::biclopsUpdateLoop()
{
	bool updated = false;

	//cout << "Firing up Biclops update thread." << endl;
// Publish Position & Speed
     
      
	while(!_threadKill)
	{

		for(int jnt = 0; jnt < BICLOPS_NUM_JOINTS; jnt++)
		{
			if(joint_commands[jnt] != joint_last_set[jnt])
			{
				pthread_mutex_lock(&position_mutex);
				double val = joint_commands[jnt];
				joint_last_set[jnt] = val;
				updated = true;
				pthread_mutex_unlock(&position_mutex);

				double unit = radiansToUnit(val, jnt);
	
				PMDAxisControl::Profile p;
				PMDAxisControl *axis = _biclops->GetAxis(jnt);

				axis->SetPosition(axis->UnitsToCounts(unit));
				axis->GetProfile(p);
				p.pos = unit;
				axis->SetProfile(p);

				axis->Move(false, true);
			}

	       
		}

	
		if(updated)
		{
			usleep(BICLOPS_UPDATE_SLEEP_TIME_MS*1000);
			updated = false;
		}
		else
		{
			usleep(0);
		}
	};
}
Пример #6
0
int main() {
	Biclops biclops;

	// Create a debug header message for use in this example application.
	const char dbgMethod[] = "main: ";
	char dbgHdr[32];
	sprintf(dbgHdr,"BiclopsPMDTest::main:");

	// Defines which axes we want to use.
	const int axisMask = Biclops::PanMask
					   + Biclops::TiltMask;

	// Pointers to each axis (populated once controller is initialized).
	PMDAxisControl *panAxis = NULL;
	PMDAxisControl *tiltAxis = NULL;

	// Set Debug level depending on how much info you want to see about
	// the inner workings of the API. Level 2 is highest with 0 being
	// the default (i.e., no messages).
	biclops.SetDebugLevel(2);

	//setting up the Kinect
	Mat rgbMat(Size(640,480),CV_8UC3);
	Mat depthMat(Size(640,480),CV_16UC1);
	//specify path to calibration file
	char calib_filepath[] = "calib/kinect_calibration2.yml";
	//initialize the kinect
	kinect myKinect(0, calib_filepath, false);
	myKinect.start();

	coutDbg << "Initializing Biclops\n";
	if (biclops.Initialize("data/BiclopsDefault.cfg")) {

		cout << "Initializing Kinect" << endl;
		for(int i=0;i<10;i++)
		{
			myKinect.getVideo(rgbMat);
			myKinect.getDepth(depthMat);
		}

		// Initialize the Biclops unit.
		coutDbg << "Initializing Biclops\n";
		// Initialization completed successfully.
		coutDbg << "Biclops initialized\n";
		biclops.SetDebugLevel(0);

		// Get shortcut references to each axis.
		panAxis = biclops.GetAxis(Biclops::Pan);
		tiltAxis = biclops.GetAxis(Biclops::Tilt);

		coutDbg << "Begin homing sequence." << endl;
		if (biclops.HomeAxes(axisMask,true)) {

			// Get the currently defined (default) motion profiles.
			PMDAxisControl::Profile panProfile,tiltProfile;
			panAxis->GetProfile(panProfile);
			tiltAxis->GetProfile(tiltProfile);

			// Set a position to move to by modifying the respective profiles.
			// NOTE: profile values are in revolutions, so here we convert
			// from degrees (divide by 360) for readability.
			panProfile.pos = PMDUtils::DegsToRevs(panAngles[0]);
			tiltProfile.pos = PMDUtils::DegsToRevs(tiltAngles[0]);

			// Inform the controller of the new desired position.
			panAxis->SetProfile(panProfile);
			tiltAxis->SetProfile(tiltProfile);

			// Move one axis to its destination in series.
			coutDbg << "Individual axis move" << endl;
			panAxis->Move();
			tiltAxis->Move();
			coutDbg << "Individual axis move complete" << endl;

			//create folder to store the Kinect data
			int fCount = 0;
			string mainFolder = "Scan";
			std::ostringstream folderPath;
			for(;;)
			{
				folderPath.str("");
				folderPath << mainFolder << fCount;
				if(!utils::checkDirectory(folderPath.str(), false))
				{
					utils::checkDirectory(folderPath.str(), true);
					break;
				}
				fCount++;
			}

			for(int i=0;i<sizeof(tiltAngles)/sizeof(double);i++)
			{
				for(int j=0;j<sizeof(panAngles)/sizeof(double);j++)
				{

					// Pick a new destination and tell the controller.
					panProfile.pos = PMDUtils::DegsToRevs(panAngles[j]);
					panAxis->SetProfile(panProfile);
					tiltProfile.pos = PMDUtils::DegsToRevs(tiltAngles[i]);
					tiltAxis->SetProfile(tiltProfile);

					coutDbg << "Moving to Pan: " << panAngles[j] << " Tilt: " << tiltAngles[i] << endl;
					// Issue the move command for all the axes simultaneously.
					coutDbg << "Multiple axis move" << endl;
					biclops.Move(axisMask);
					coutDbg << "Multiple axis move complete" << endl;
					if(j==0)
						sleep(4);
					else
						sleep(2);

					//creating folders and subfolders to store the images
					std::ostringstream subFolderPath, rgbFolderPath, depthFolderPath;
					subFolderPath << folderPath.str() << "/P" << panAngles[j] << "T" << tiltAngles[i];
					rgbFolderPath << subFolderPath.str() << "/rgb";
					depthFolderPath << subFolderPath.str() << "/depth";
					utils::checkDirectory(subFolderPath.str(), true);
					utils::checkDirectory(rgbFolderPath.str(), true);
					utils::checkDirectory(depthFolderPath.str(), true);

					//grabbing kinect frames
					for(int k=0;k<10;k++)
					{
						myKinect.getVideo(rgbMat);
						myKinect.getDepth(depthMat);

						//save kinect frames to folder
						std::ostringstream rgbPath, depthPath;
						rgbPath << rgbFolderPath.str() << "/rgb" << k << ".png";
						depthPath << depthFolderPath.str() << "/depth" << k << ".png";
						cv::imwrite(rgbPath.str().c_str(),rgbMat);
						cv::imwrite(depthPath.str().c_str(),depthMat);
					}
				}
			}

			//back to home position
			panProfile.pos = PMDUtils::DegsToRevs(0);
			panAxis->SetProfile(panProfile);
			tiltProfile.pos = PMDUtils::DegsToRevs(0);
			tiltAxis->SetProfile(tiltProfile);
			biclops.Move(axisMask);

			// Turn off motor amps to conserve power.
			panAxis->DisableAmp();
			tiltAxis->DisableAmp();
		}
		else {
			coutDbg << "initialization failed" << endl;
		}
	}

	// Stop the Kinect
	myKinect.stop();

	return 0;
}