コード例 #1
0
ファイル: asshat.c プロジェクト: solidblu1992/AsshatGTK
static gboolean pwm_timer_event(GtkWidget *widget)
{
	double t = getDoubleTime() - pwm.lastUpdate;

	//Always On
	if (pwm.dc >= 100.0f)
	{
		digitalWrite(pwm.pin, HIGH);
		pwm.lastUpdate = getDoubleTime();
	}
	//Always Off
	else if (pwm.dc <= 0.0f)
	{
		digitalWrite(pwm.pin, LOW);
		pwm.lastUpdate = getDoubleTime();
	}
	//Currently On
	else if (pwm.state && t > (pwm.period * pwm.dc / 100.f))
	{
		digitalWrite(pwm.pin, LOW);
		pwm.state = 0;
	}
	else if (!pwm.state && (t > pwm.period))
	{
		digitalWrite(pwm.pin, HIGH);
		pwm.state = 1;
		pwm.lastUpdate = getDoubleTime();
	} 

	return 1;
}
コード例 #2
0
ファイル: STAT_timer.C プロジェクト: LLNL/STAT
STAT_timer STAT_timer::operator-(STAT_timer& statTimer)
{
    STAT_timer retval;
    double doubleTime = getDoubleTime() - statTimer.getDoubleTime();
    retval.setTime(doubleTime);
    return retval;
}
コード例 #3
0
ファイル: md3view.cpp プロジェクト: UberGames/MD3View
/*
initializes the viewer global state data
*/
void init_mdview(const char* lpcommandline)
{
	mdview.dFOV = 90.0f;
	mdview.iLODLevel = 0;
	mdview.iSkinNumber = 0;
	mdview.bAxisView = false;
	reset_viewpos();
	mdview.animSpeed = 0.1;	// so 1/this = 10 = 10FPS
	mdview.timeStamp1 = getDoubleTime();
	mdview.texMode = TEX_FILTERED;
	mdview.faceSide = GL_CCW;
	mdview.animate = false;
	mdview.interpolate = true;
	mdview.bUseAlpha = false;	// default this to OFF since artists keep making 32 bit textures with no alpha, then complain about pieces missing!!!!
	mdview.bBBox = false;
	mdview._R = mdview._G = mdview._B = 256/5;	// dark grey
	mdview.bAnimCFGLoaded = false;
	mdview.bAnimIsMultiPlayerFormat = false;

	// allocate texture resource manager
	mdview.textureRes = new NodeDictionaryInfo( new StrKeyComparatorInfo() );
	mdview.topTextureBind = 0;

	// allocate model list
	mdview.modelList = new NodeSequenceInfo();

//	strcpy( mdview.basepath, "/");

	if ( lpcommandline != NULL && lpcommandline[0] != '\0' ) { //got an initial dir 
		char	szDirName[256]={0};
		strcpy (szDirName, lpcommandline );
		char *p = strrchr(szDirName,'\\');	//back up off the filename.
		if (p) {
			*p=0;
		}
		SetCurrentDirectory( szDirName);	//this will make future open dialogs in the same dir as the command line file
		strcpy( mdview.basepath, szDirName);
	} else {
#if 1
		strcpy( mdview.basepath, "w:\\game\\base\\models\\");	// this is just to default the 1st use of OpenFileDialog, and doesn't affect anything else once the app gets going
		SetCurrentDirectory( mdview.basepath );
#else
		char	szDirName[256]={0};
		GetModuleFileName(NULL, szDirName, 256);
		char *p = strrchr(szDirName,'\\');	//back up off the filename.
		if (p) {
			*p=0;
		}
		SetCurrentDirectory( szDirName );
		strcpy( mdview.basepath, szDirName);
#endif
	}
}
コード例 #4
0
ファイル: asshat.c プロジェクト: solidblu1992/AsshatGTK
static void btnStepResponseOnClick(GtkWidget *widget, gpointer data)
{
	if (gtk_toggle_button_get_active((GtkToggleButton*) btnStepResponse))
	{
		ctrl.StepResponseMode = 1;
		CTRL_PID_Update(&ctrl, getDoubleTime());
		
	}
	else
	{
		ctrl.StepResponseMode = 0;
	}

	ctrl.lastErr[0] = 0.0f;
	ctrl.lastErr[1] = 0.0f;
	ctrl.lastCV[0] = 0.0f;
}
コード例 #5
0
    Eigen::Affine3d NDTFuserHMT::update(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud)
    {
	if(!isInit){
	    fprintf(stderr,"NDT-FuserHMT: Call Initialize first!!\n");
	    return Tnow;
	}
	Todom = Todom * Tmotion; //we track this only for display purposes!
	double t0=0,t1=0,t2=0,t3=0,t4=0,t5=0,t6=0;
	///Set the cloud to sensor frame with respect to base
	lslgeneric::transformPointCloudInPlace(sensor_pose, cloud);
	t0 = getDoubleTime();
	///Create local map
	lslgeneric::NDTMap ndlocal(new lslgeneric::LazyGrid(resolution));
	ndlocal.guessSize(0,0,0,sensor_range,sensor_range,map_size_z);
	ndlocal.loadPointCloud(cloud,sensor_range);
	ndlocal.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
	//pass through ndlocal and set all cells with vertically pointing normals to non-gaussian :-O
	/*SpatialIndex *index = ndlocal.getMyIndex();
	  typename SpatialIndexctorItr it = index->begin();
	  while (it != index->end())
	  {
	  NDTCell *cell = dynamic_cast<NDTCell*> (*it);
	  if(cell!=NULL)
	  {
	  if(cell->hasGaussian_)
	  {
	  if(cell->getClass() == NDTCell::HORIZONTAL) {
	  cell->hasGaussian_ = false;
	  }
	  }
	  }
	  it++;
	  }*/

	t1 = getDoubleTime();
	Eigen::Affine3d Tinit = Tnow * Tmotion;
	if(disableRegistration) {
	    Tnow = Tinit;
	    lslgeneric::transformPointCloudInPlace(Tnow, cloud);
	    Eigen::Affine3d spose = Tnow*sensor_pose;
	    map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
	    if(visualize) //&&ctr%20==0) 
	    {
#ifndef NO_NDT_VIZ
		if(ctr%50==0) {

		    viewer->plotNDTSAccordingToOccupancy(-1,map); 
		    //viewer->plotLocalNDTMap(cloud,resolution); 
		}
		viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
		viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
		viewer->displayTrajectory();
		viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
		viewer->repaint();	
#endif
	    }
	    ctr++;
	    return Tnow;
	}

	if(doMultires) {
	    //create two ndt maps with resolution = 3*resolution (or 5?)
	    lslgeneric::NDTMap ndlocalLow(new lslgeneric::LazyGrid(3*resolution));
	    ndlocalLow.guessSize(0,0,0,sensor_range,sensor_range,map_size_z);
	    ndlocalLow.loadPointCloud(cloud,sensor_range);
	    ndlocalLow.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);

	    lslgeneric::NDTMap mapLow(new lslgeneric::LazyGrid(3*resolution));
	    //add distros
	    double cx,cy,cz;
	    if(!map->getCentroid(cx, cy, cz)){
		fprintf(stderr,"Centroid NOT Given-abort!\n");
	    }
	    mapLow.initialize(cx,cy,cz,3*map_size_x,3*map_size_y,map_size_z);

	    std::vector<lslgeneric::NDTCell*> ndts;
	    ndts = map->getAllCells(); //this copies cells?

	    for(int i=0; i<ndts.size(); i++)	
	    {
		NDTCell *cell = ndts[i];
		if(cell!=NULL)
		{
		    if(cell->hasGaussian_)
		    {
			Eigen::Vector3d m = cell->getMean();	
			Eigen::Matrix3d cov = cell->getCov();
			unsigned int nump = cell->getN();
			mapLow.addDistributionToCell(cov, m,nump);
		    }
		}
		delete cell;
	    }
	    //do match
	    if(matcher2D.match( mapLow, ndlocalLow,Tinit,true)){
		//if success, set Tmotion to result
		t2 = getDoubleTime();
		//std::cout<<"success: new initial guess! t= "<<t2-t1<<std::endl;
	    } else {
		Tinit = Tnow * Tmotion;
	    }	    
	}

	if(be2D) {
	    t2 = getDoubleTime();
	    if(matcher2D.match( *map, ndlocal,Tinit,true) || fuseIncomplete){
		t3 = getDoubleTime();
		Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;
		if((diff.translation().norm() > max_translation_norm || 
			    diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
		    fprintf(stderr,"****  NDTFuserHMT -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
		    Tnow = Tnow * Tmotion;
		}else{
		    Tnow = Tinit;
		    lslgeneric::transformPointCloudInPlace(Tnow, cloud);
		    Eigen::Affine3d spose = Tnow*sensor_pose;
		    Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
		    if(diff_fuse.translation().norm() > translation_fuse_delta ||
			    diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
		    {
			//std::cout<<"F: "<<spose.translation().transpose()<<" "<<spose.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
			t4 = getDoubleTime();
			//TSV: originally this!
			//map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
			map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
			t5 = getDoubleTime();
			//map->addPointCloud(spose.translation(),cloud, 0.06, 25);
			//map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
			//t4 = getDoubleTime();
			//std::cout<<"match: "<<t3-t2<<" addPointCloud: "<<t5-t4<<" ndlocal "<<t1-t0<<" total: "<<t5-t0<<std::endl;
			Tlast_fuse = Tnow;
			if(visualize) //&&ctr%20==0) 
			{
#ifndef NO_NDT_VIZ
			    if(ctr%30==0) {
				viewer->plotNDTSAccordingToOccupancy(-1,map); 
				//viewer->plotLocalNDTMap(cloud,resolution); 
			    }
			    viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
			    viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
			    viewer->displayTrajectory();
			    viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
			    viewer->repaint();
			    //viewer->win3D->process_events();
#endif
            }

			ctr++;
		    }
		}
	    }else{
		t3 = getDoubleTime();
		Tnow = Tnow * Tmotion;
	    }

	}
	else
	{

	    t2 = getDoubleTime();
	    if(matcher.match( *map, ndlocal,Tinit,true) || fuseIncomplete){
		t3 = getDoubleTime();
		Eigen::Affine3d diff = (Tnow * Tmotion).inverse() * Tinit;

		if((diff.translation().norm() > max_translation_norm || 
			    diff.rotation().eulerAngles(0,1,2).norm() > max_rotation_norm) && checkConsistency){
		    fprintf(stderr,"****  NDTFuserHMT -- ALMOST DEFINATELY A REGISTRATION FAILURE *****\n");
		    Tnow = Tnow * Tmotion;
		    //save offending map:
		    //map->writeToJFF("map.jff");
		    //ndlocal.writeToJFF("local.jff");
		}else{
		    Tnow = Tinit;
		    //Tnow = Tnow * Tmotion;
		    lslgeneric::transformPointCloudInPlace(Tnow, cloud);
		    Eigen::Affine3d spose = Tnow*sensor_pose;
		    Eigen::Affine3d diff_fuse = Tlast_fuse.inverse()*Tnow;
		    if(diff_fuse.translation().norm() > translation_fuse_delta ||
			    diff_fuse.rotation().eulerAngles(0,1,2).norm() > rotation_fuse_delta)
		    {
			//std::cout<<"F: "<<spose.translation().transpose()<<" "<<spose.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
			t4 = getDoubleTime();
			map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 1250, map_size_z/2, 0.06);
			t5 = getDoubleTime();
			//map->addPointCloudMeanUpdate(spose.translation(),cloud,localMapSize, 1e5, 25, 2*map_size_z, 0.06);
			//map->addPointCloud(spose.translation(),cloud, 0.06, 25);
			//map->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE, 1e5, 255, spose.translation(), 0.1);
			//t4 = getDoubleTime();
			//std::cout<<"match: "<<t3-t2<<" addPointCloud: "<<t5-t4<<" ndlocal "<<t1-t0<<" total: "<<t5-t0<<std::endl;
			Tlast_fuse = Tnow;
			if(visualize) //&&ctr%20==0) 
			{
#ifndef NO_NDT_VIZ
			    if(ctr%2==0) {
				viewer->plotNDTSAccordingToOccupancy(-1,map); 
				//viewer->plotLocalNDTMap(cloud,resolution); 
			    }
			    viewer->addTrajectoryPoint(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+0.2,0,1,0);
			    viewer->addTrajectoryPoint(Todom.translation()(0),Todom.translation()(1),Todom.translation()(2)+0.2,0.5,0,0.5);
			    viewer->displayTrajectory();
			    viewer->setCameraPointing(Tnow.translation()(0),Tnow.translation()(1),Tnow.translation()(2)+3);
			    viewer->repaint();
                viewer->win3D->process_events();
#endif
            }
			ctr++;
		    }
		}
	    }else{
		t3 = getDoubleTime();
		Tnow = Tnow * Tmotion;
	    }
	}

	t6 = getDoubleTime();
	if(fAddTimes!=NULL) {
	    fprintf(fAddTimes,"%lf %lf %lf\n",t3-t2,t5-t4,t6-t0);
	    fflush(fAddTimes);
	}

	return Tnow;
    }
コード例 #6
0
ファイル: 3d_ndt_mcl.cpp プロジェクト: vbillys/velo_mapping
void NDTMCL3D::updateAndPredictEff(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud, double subsample_level){
    if(subsample_level < 0 || subsample_level > 1) subsample_level = 1;

    Eigen::Vector3d tr = Tmotion.translation();
    Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);

    double time_start = getDoubleTime();
    //pf.predict(Tmotion, tr[0]*0.1, tr[1]*0.1, tr[2]*0.1, rot[0]*0.1, rot[1]*0.1, rot[2]*0.1);
    if(rot[2]<(0.5 * M_PI/180.0) && tr[0]>=0){ 
	//pf.predict(Tmotion, tr[0]*0.2 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.001);
	pf.predict(Tmotion, tr[0]*0.2 + 0.105, tr[1]*0.1+ 0.105, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.031);
    }else if(tr[0]>=0){
	//pf.predict(Tmotion,tr[0]*0.5 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.4+0.001);
	pf.predict(Tmotion,tr[0]*0.5 + 0.095, tr[1]*0.1+ 0.095, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.4+0.061);
    }else{
	//pf.predict(Tmotion, tr[0]*0.2 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.001);
	pf.predict(Tmotion, tr[0]*0.2 + 0.055, tr[1]*0.1+ 0.055, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.021);
    }


    double t_pred = getDoubleTime() - time_start;	

    std::cerr<<"cloud points "<<cloud.points.size()<<" res :"<<resolution<<" sres: "<<resolution_sensor<<std::endl;
    lslgeneric::NDTMap local_map(new lslgeneric::LazyGrid(resolution));
    //local_map.guessSize(0,0,0,30,30,10); //sensor_range,sensor_range,map_size_z);
    local_map.loadPointCloud(cloud);//,30); //sensor_range);
    local_map.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);

    /*lslgeneric::NDTMap<PointT> local_map(new lslgeneric::LazyGrid<PointT>(resolution_sensor));		
      local_map.addPointCloudSimple(cloud);
    //local_map.computeNDTCells();
    local_map.computeNDTCellsSimple();
     */
    std::vector<lslgeneric::NDTCell*> ndts0 = local_map.getAllCells();
    std::vector<lslgeneric::NDTCell*> ndts;
    std::cerr<<"ndts: "<<ndts0.size()<<std::endl;

    if(subsample_level != 1) {
	srand((int)(t_pred*10000));
	for(int i=0; i<ndts0.size(); ++i) {
	    double p = ((double)rand())/RAND_MAX;
	    if(p < subsample_level) {
		ndts.push_back(ndts0[i]);
	    } else {
		delete ndts0[i];
	    }
	}	
    } else {
	ndts = ndts0;
    }
    std::cerr<<"resampled ndts: "<<ndts.size()<<std::endl;

    int Nn = 0;
    //		#pragma omp parallel for
    double t_pseudo = 0;
#pragma omp parallel num_threads(4)
    {
#pragma omp for
	for(int i=0;i<pf.size();i++){
	    Eigen::Affine3d T = pf.pcloud[i].T;


	    //ndts = local_map.pseudoTransformNDT(T);
	    double score=1;

	    if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
	    Nn = ndts.size();

	    for(int n=0;n<ndts.size();n++){
		Eigen::Vector3d m = T*ndts[n]->getMean();	

		if(m[2]<zfilt_min) continue;

		lslgeneric::NDTCell *cell;
		pcl::PointXYZ p;
		p.x = m[0];p.y=m[1];p.z=m[2];

		if(map.getCellAtPoint(p,cell)){
		    //if(map.getCellForPoint(p,cell)){
		    if(cell == NULL) continue;
		    if(cell->hasGaussian_){
			Eigen::Matrix3d covCombined = cell->getCov() + T.rotation()*ndts[n]->getCov() *T.rotation().transpose();
			Eigen::Matrix3d icov;
			bool exists;
			double det = 0;
			covCombined.computeInverseAndDetWithCheck(icov,det,exists);
			if(!exists) continue;
			double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
			if(l*0 != 0) continue;
			score += 0.1 + 0.9 * exp(-0.05*l/2.0);
		    }else{
		    }
		}
		}

		pf.pcloud[i].lik = score;


	    }
	}///#pragma
	for(unsigned int j=0;j<ndts.size();j++){
	    delete ndts[j];
	}


	pf.normalize();


	if(forceSIR){	
	    fprintf(stderr, "forceSIR(%d) ",forceSIR);
	    pf.SIRUpdate();
	}else{

	    double varP=0;
	    for(int i = 0; i<pf.size();i++){
		varP += (pf.pcloud[i].p - 1.0/pf.size())*(pf.pcloud[i].p - 1.0/pf.size());
	    }
	    varP /= pf.size();
	    varP = sqrt(varP); 
	    fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d) (t_pred = %.3lf t_pseudo=%.3lf)",varP,pf.size(), Nn, t_pred,t_pseudo);
	    if(varP > 0.006 || sinceSIR >25){
		fprintf(stderr,"-SIR- ");
		sinceSIR = 0;
		pf.SIRUpdate();
	    }else{
		sinceSIR++;
	    }

	}
    }
コード例 #7
0
ファイル: 3d_ndt_mcl.cpp プロジェクト: vbillys/velo_mapping
void NDTMCL3D::updateAndPredict(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud){
    Eigen::Vector3d tr = Tmotion.translation();
    Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);

    double t_start = getDoubleTime();
    pf.predict(Tmotion, tr[0]*0.1, tr[1]*0.1, tr[2]*0.1, rot[0]*0.1, rot[1]*0.1, rot[2]*0.1);
    double t_pred = getDoubleTime() - t_start;	

    //pf.predict(mcl::pose(tr[0],tr[1],rot[2]), mcl::pose(tr[0]*0.1 + 0.005,tr[1]*0.1+ 0.005,rot[2]*0.1+0.001));

    lslgeneric::NDTMap local_map(new lslgeneric::LazyGrid(resolution_sensor));
    std::cerr<<"cloud points "<<cloud.points.size()<<std::endl;
    local_map.addPointCloudSimple(cloud);
    local_map.computeNDTCells();
    //local_map.computeNDTCells(CELL_UPDATE_MODE_STUDENT_T);
    int Nn = 0;
    //		#pragma omp parallel for
    double t_pseudo = 0;

    for(int i=0;i<pf.size();i++){
	Eigen::Affine3d T = pf.pcloud[i].T;

	std::vector<lslgeneric::NDTCell*> ndts;
	double tictime = getDoubleTime();
	ndts = local_map.pseudoTransformNDT(T);
	t_pseudo += getDoubleTime()-tictime;
	double score=1;

	if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
	Nn = ndts.size();

	for(int n=0;n<ndts.size();n++){
	    Eigen::Vector3d m = ndts[n]->getMean();	
	    if(m[2]<zfilt_min) continue;

	    lslgeneric::NDTCell *cell;
	    pcl::PointXYZ p;
	    p.x = m[0];p.y=m[1];p.z=m[2];

	    if(map.getCellAtPoint(p,cell)){
		//if(map.getCellForPoint(p,cell)){
		if(cell == NULL) continue;
		if(cell->hasGaussian_){
		    Eigen::Matrix3d covCombined = cell->getCov() + ndts[n]->getCov();
		    Eigen::Matrix3d icov;
		    bool exists;
		    double det = 0;
		    covCombined.computeInverseAndDetWithCheck(icov,det,exists);
		    if(!exists) continue;
		    double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
		    if(l*0 != 0) continue;
		    score += 0.1 + 0.9 * exp(-0.05*l/2.0);
		}else{
		}
	    }
	    }

	    pf.pcloud[i].lik = score;
	    for(unsigned int j=0;j<ndts.size();j++){
		delete ndts[j];
	    }

	}

	pf.normalize();


	if(forceSIR){	
	    fprintf(stderr, "forceSIR(%d) ",forceSIR);
	    pf.SIRUpdate();
	}else{

	    double varP=0;
	    for(int i = 0; i<pf.size();i++){
		varP += (pf.pcloud[i].p - 1.0/pf.size())*(pf.pcloud[i].p - 1.0/pf.size());
	    }
	    varP /= pf.size();
	    varP = sqrt(varP); 
	    fprintf(stderr,"Var P=%lf (Npf=%d, Nm=%d) (t_pred = %.3lf t_pseudo=%.3lf)",varP,pf.size(), Nn, t_pred,t_pseudo);
	    if(varP > 0.006 || sinceSIR >25){
		fprintf(stderr,"-SIR- ");
		sinceSIR = 0;
		pf.SIRUpdate();
	    }else{
		sinceSIR++;
	    }

	}
    }
コード例 #8
0
ファイル: asshat.c プロジェクト: solidblu1992/AsshatGTK
void LoadDefaultParameters(Parameters_t *p, char * filename)
{
	FILE *f;
	char ch;
	char parameter_name[256];
	int i = 0;	
	if (0 && (f = fopen("parameters.csv","r")))
	{
		while ((ch = fgetc(f)) != EOF)
		{
			if (ch == ',')
			{
				//End of parameter name
				buffer[i] = '\0';
				strcpy(parameter_name, buffer);				
				printf("Loading Parameter '%s'...", parameter_name);
				i = 0;
			}
			else if (ch == '\n')
			{
				//End of parameter value			
				buffer[i] = '\0';
				printf("Value = ('%s')\n", buffer);
				i = 0;

				//Store Parameters
				if (strcmp("Kc", buffer) == 0)
				{
					
				}
				else if (strcmp("TDevMax", parameter_name) == 0)
				{
				
				}
			}
			else
			{
				buffer[i] = ch;
				i++;
			}
		}
	}
	else
	{
		parameter_set.useDependentGains = 1;
		parameter_set.Kc = 150.f;
		parameter_set.Ti = 0.0;
		parameter_set.Td = 0.0;
		parameter_set.Kp = 150.f;
		parameter_set.Ki = 0.0;
		parameter_set.Kd = 0.1f;
		parameter_set.Tsp = 156.0;
		parameter_set.Tbias = 0.55;
		parameter_set.Tdev = 7.0;
		parameter_set.Wcold = 1.0;
		parameter_set.Whot = 1.0;
		parameter_set.CVlow = 0.0;
		parameter_set.CVhigh = 100.0;
		parameter_set.ControllerActive = 0;
		parameter_set.lastPWMTime = getDoubleTime();
		parameter_set.lastPrintTime = getDoubleTime();
	}
}
コード例 #9
0
ファイル: asshat.c プロジェクト: solidblu1992/AsshatGTK
static gboolean update_event(GtkWidget *widget)
{
	int deviationFault;
	double Tcold, Thot;

	//Get Temperatures
	if (!digitalRead(MAX31865_DR0_PIN))
	{
		Tcold = MAX31865_GetTemperature(MAX31865_CS0_PIN) + parameter_set.Tbias;
	}
	if (!digitalRead(MAX31865_DR1_PIN))
	{
		Thot = MAX31865_GetTemperature(MAX31865_CS1_PIN) + parameter_set.Tbias;
	}
		
	ctrl.pv = (Tcold*parameter_set.Wcold+Thot*parameter_set.Whot)/(parameter_set.Wcold+parameter_set.Whot);
	CTRL_PID_Update(&ctrl, getDoubleTime());

	//Update PWM
	//NOTE: THE PWM IS ACTIVE LOW
	//PWM Always Off (zero duty cycle, controller off, or deviation)
	deviationFault = ((Thot-Tcold)*(Thot-Tcold) > (parameter_set.Tdev*parameter_set.Tdev) ? 1 : 0);
	
	if (deviationFault || !parameter_set.ControllerActive || ctrl.StepResponseMode)
	{
		pwm.dc = 100.0f;
	}
	else
	{
		pwm.dc = 100.0f - ctrl.CVdc;
	}

	//Data Printer (Print every second)
	if ((getDoubleTime() - parameter_set.lastPrintTime) > 1.0 || (ctrl.StepResponseMode && !lastStepResponseMode))
	{
		sprintf(buffer, "%f", Tcold);
		gtk_label_set_text((GtkLabel *)lblColdLegTemp, (gchar *)buffer);
		sprintf(buffer, "%f", Thot);
		gtk_label_set_text((GtkLabel *)lblHotLegTemp, (gchar *)buffer);
		sprintf(buffer, "%f", ctrl.pv);
		gtk_label_set_text((GtkLabel *)lblTpv, (gchar *)buffer);
		sprintf(buffer, "%f", 100.f - pwm.dc);
		gtk_label_set_text((GtkLabel *)lblHeatingElementDC, (gchar *)buffer);
		sprintf(buffer, "%f", (parameter_set.Tsp - ctrl.pv));
		gtk_label_set_text((GtkLabel *)lblError, (gchar *)buffer);

		if (deviationFault)
		{
			sprintf(buffer,"%s","Deviation Fault");
		}
		else
		{
			sprintf(buffer, "%s", "None");
		}
		gtk_label_set_text((GtkLabel *)lblAlarm, (gchar*) buffer);

		if (ctrl.StepResponseMode)
		{
			printf("%f ms, SP=1.0, CVdc=%f\n", getDoubleTime(), ctrl.CVdc);
		}
		else		
		{
			printf("%f ms, CVdc=%f, SP=%f, Tave=%f, Tcold=%f °F, Thot=%f °F\n", getDoubleTime(), pwm.dc, ctrl.sp, ctrl.pv, Tcold, Thot);
		}

		parameter_set.lastPrintTime = getDoubleTime();	
	}
	lastStepResponseMode = ctrl.StepResponseMode;

	return 1;
}
コード例 #10
0
ファイル: asshat.c プロジェクト: solidblu1992/AsshatGTK
int main(int argc, char *argv[])
{
	//Initialize GTK
	gtk_init(&argc, &argv);
	builder = gtk_builder_new();
	gtk_builder_add_from_file (builder, "main.glade", NULL);

	window = GTK_WIDGET (gtk_builder_get_object(builder, "window"));
	lblColdLegTemp = GTK_WIDGET (gtk_builder_get_object(builder, "lblColdLegTemp"));
	lblHotLegTemp = GTK_WIDGET (gtk_builder_get_object(builder, "lblHotLegTemp"));
	lblHeatingElementDC = GTK_WIDGET (gtk_builder_get_object(builder, "lblHeatingElementDC"));
	lblTpv = GTK_WIDGET (gtk_builder_get_object(builder, "lblTpv"));
	lblAlarm = GTK_WIDGET (gtk_builder_get_object(builder, "lblAlarm"));
	lblError = GTK_WIDGET (gtk_builder_get_object(builder, "lblError"));
	rbDependentGains = GTK_WIDGET (gtk_builder_get_object(builder, "rbDependentGains"));
	rbIndependentGains = GTK_WIDGET (gtk_builder_get_object(builder, "rbIndependentGains"));
	txtKc = GTK_WIDGET (gtk_builder_get_object(builder, "txtKc"));
	txtTi = GTK_WIDGET (gtk_builder_get_object(builder, "txtTi"));
	txtTd = GTK_WIDGET (gtk_builder_get_object(builder, "txtTd"));
	txtKp = GTK_WIDGET (gtk_builder_get_object(builder, "txtKp"));
	txtKi = GTK_WIDGET (gtk_builder_get_object(builder, "txtKi"));
	txtKd = GTK_WIDGET (gtk_builder_get_object(builder, "txtKd"));
	txtTsp = GTK_WIDGET (gtk_builder_get_object(builder, "txtTsp"));
	txtTdev = GTK_WIDGET (gtk_builder_get_object(builder, "txtTdev"));
	txtTbias = GTK_WIDGET (gtk_builder_get_object(builder, "txtTbias"));
	txtWcold = GTK_WIDGET (gtk_builder_get_object(builder, "txtWcold"));
	txtWhot = GTK_WIDGET (gtk_builder_get_object(builder, "txtWhot"));
	txtCVlow = GTK_WIDGET (gtk_builder_get_object(builder, "txtCVlow"));
	txtCVhigh = GTK_WIDGET (gtk_builder_get_object(builder, "txtCVhigh"));

	//Default Parameter Set (load from file or default if they don't exist)
	LoadDefaultParameters(&parameter_set, "parameters.csv");

	//Update Parameter Text Boxes to Reflect Defaults
	sprintf(buffer, "%f", parameter_set.Kc);
	gtk_entry_set_text ((GtkEntry*) txtKc, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Ti);
	gtk_entry_set_text ((GtkEntry*) txtTi, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Td);
	gtk_entry_set_text ((GtkEntry*) txtTd, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Kp);
	gtk_entry_set_text ((GtkEntry*) txtKp, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Ki);
	gtk_entry_set_text ((GtkEntry*) txtKi, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Kd);
	gtk_entry_set_text ((GtkEntry*) txtKd, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Tsp);
	gtk_entry_set_text ((GtkEntry*) txtTsp, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Tdev);
	gtk_entry_set_text ((GtkEntry*) txtTdev, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Tbias);
	gtk_entry_set_text ((GtkEntry*) txtTbias, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Wcold);
	gtk_entry_set_text ((GtkEntry*) txtWcold, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.Whot);
	gtk_entry_set_text ((GtkEntry*) txtWhot, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.CVlow);
	gtk_entry_set_text ((GtkEntry*) txtCVlow, (gchar *) buffer);
	sprintf(buffer, "%f", parameter_set.CVhigh);
	gtk_entry_set_text ((GtkEntry*) txtCVhigh, (gchar *) buffer);

	btnUpdateParameters = GTK_WIDGET (gtk_builder_get_object(builder, "btnUpdateParameters"));
	g_signal_connect (btnUpdateParameters, "clicked", G_CALLBACK(btnUpdateParametersOnClick), NULL);
	
	btnControllerActive = GTK_WIDGET (gtk_builder_get_object(builder, "btnControllerActive"));
	g_signal_connect (btnControllerActive, "clicked", G_CALLBACK(btnControllerActiveOnClick), NULL);

	btnRTDInit = GTK_WIDGET (gtk_builder_get_object(builder, "btnRTDInit"));
	g_signal_connect (btnRTDInit, "clicked", G_CALLBACK(btnRTDInitOnClick), NULL);

	btnStepResponse = GTK_WIDGET (gtk_builder_get_object(builder, "btnStepResponse"));
	g_signal_connect (btnStepResponse, "clicked", G_CALLBACK(btnStepResponseOnClick), NULL);

	//Update Controller Active Button Label to Default
	gtk_button_set_label(GTK_BUTTON(btnControllerActive), "Controller Inactive");
	gtk_toggle_button_set_active((GtkToggleButton*) btnControllerActive, 0);

	gtk_toggle_button_set_active((GtkToggleButton*) btnStepResponse, 0);
	ctrl.StepResponseMode = 0;
	lastStepResponseMode = 1;

	gtk_builder_connect_signals(builder, NULL);
	g_object_unref(G_OBJECT(builder));

	g_signal_connect(window, "delete-event", G_CALLBACK(delete_event), NULL);
	g_signal_connect(window, "destroy", G_CALLBACK(destroy), NULL);
		
	//Update Controller every 20ms and PWM every 1ms
	g_timeout_add(20, (GSourceFunc) update_event, (gpointer) window); 	
	g_timeout_add(1, (GSourceFunc) pwm_timer_event, (gpointer) window); 
	gtk_widget_show (window);

	btnUpdateParametersOnClick(NULL, NULL);

	//Initialize WiringPi
	if(wiringPiSetupGpio() < 0)
	{
		return -1;
	}

	//Configure SPI Bus
	spi_init();

	//Configure MAX31865
	pinMode(MAX31865_DR0_PIN, INPUT);
	pinMode(MAX31865_DR1_PIN, INPUT);
	pinMode(MAX31865_CS0_PIN, OUTPUT);
	pinMode(MAX31865_CS1_PIN, OUTPUT);

	btnRTDInitOnClick(NULL, NULL);

	//Initialize PWM
	pwm.pin = HeatingElement_PIN;

	pinMode(pwm.pin, OUTPUT);
	digitalWrite(pwm.pin, HIGH);
	pwm.state = 1;
	pwm.dc = 0.0f;
	pwm.period = 0.020f; //20 ms
	pwm.lastUpdate = getDoubleTime();

	//Run GTK Loop
	gtk_main();

	return 0;
}
コード例 #11
0
ファイル: asshat.c プロジェクト: solidblu1992/AsshatGTK
//GTK Callbacks
static void btnUpdateParametersOnClick(GtkWidget *widget, gpointer data)
{
	parameter_set.Kc = strtof(gtk_entry_get_text ((GtkEntry*) txtKc), NULL);
	parameter_set.Ti = strtof(gtk_entry_get_text ((GtkEntry*) txtTi), NULL);
	parameter_set.Td = strtof(gtk_entry_get_text ((GtkEntry*) txtTd), NULL);
	parameter_set.Kp = strtof(gtk_entry_get_text ((GtkEntry*) txtKp), NULL);
	parameter_set.Ki = strtof(gtk_entry_get_text ((GtkEntry*) txtKi), NULL);
	parameter_set.Kd = strtof(gtk_entry_get_text ((GtkEntry*) txtKd), NULL);
	parameter_set.Tdev = strtof(gtk_entry_get_text ((GtkEntry*) txtTdev), NULL);
	parameter_set.Tsp = strtof(gtk_entry_get_text ((GtkEntry*) txtTsp), NULL);
	parameter_set.Tbias = strtof(gtk_entry_get_text ((GtkEntry*) txtTbias), NULL);
	parameter_set.Wcold = strtof(gtk_entry_get_text ((GtkEntry*) txtWcold), NULL);
	parameter_set.Whot = strtof(gtk_entry_get_text ((GtkEntry*) txtWhot), NULL);
	parameter_set.CVhigh = strtof(gtk_entry_get_text ((GtkEntry*) txtCVhigh), NULL);
	parameter_set.CVlow = strtof(gtk_entry_get_text ((GtkEntry*) txtCVlow), NULL);
	parameter_set.useDependentGains = gtk_toggle_button_get_active((GtkToggleButton*) rbDependentGains);
	
	//Update Gains
	if (parameter_set.useDependentGains)
	{
		//Td == 0 means no derivative
		//Taking Ti == 0 to mean no Integrator (Really Ti -> inf)		

		//P Controller
		if (parameter_set.Ti == 0.0 && parameter_set.Td == 0.0)
		{
			ctrl.Kp = parameter_set.Kc;
			ctrl.Ki = 0.0;
			ctrl.Kd = 0.0;
		}
		//PI Controller
		else if (parameter_set.Td == 0.0)
		{
			ctrl.Kp = parameter_set.Kc;
			ctrl.Ki = parameter_set.Kc / parameter_set.Ti;
			ctrl.Kd = 0.0;
		}
		//PD Controller
		else if (parameter_set.Ti == 0.0)
		{
			ctrl.Kp = parameter_set.Kc;
			ctrl.Ki = 0.0;
			ctrl.Kd = parameter_set.Kc * parameter_set.Kd;
		}
		//PID Controller
		else
		{
			ctrl.Kp = parameter_set.Kc * (parameter_set.Ti + parameter_set.Td) / parameter_set.Ti;
			ctrl.Ki = parameter_set.Kc / (parameter_set.Ti + parameter_set.Td);
			ctrl.Kd = parameter_set.Kc * (parameter_set.Ti * parameter_set.Td) / (parameter_set.Ti + parameter_set.Td);
		}
	}
	else
	{
		ctrl.Kp = parameter_set.Kp;
		ctrl.Ki = parameter_set.Ki;
		ctrl.Kd = parameter_set.Kd;
	}

	//Update Controller
	ctrl.sp = parameter_set.Tsp;
	ctrl.DClow = parameter_set.CVlow;
	ctrl.DChigh = parameter_set.CVhigh;
	ctrl.lastUpdate = getDoubleTime();
}