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;
}
STAT_timer STAT_timer::operator-(STAT_timer& statTimer)
{
STAT_timer retval;
double doubleTime = getDoubleTime() - statTimer.getDoubleTime();
retval.setTime(doubleTime);
return retval;
}
/*
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
}
}
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;
}
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;
}
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++;
}
}
}
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++;
}
}
}
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();
}
}
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;
}
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(¶meter_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;
}
//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();
}