void init_Control (int M,
int fbeta,
int fwmega,
double bmin,
double bmax,
double dbeta,
double wmin,
double wmax,
double dw,
double qx,
double qy,
double mu,
double kin,
double alpha,
Control * control)
{
control->dat = (Data *) malloc (sizeof (Data));
init_Data (M, qx, qy, wmin, bmin, mu, kin, alpha, control->dat);
control->bmin = bmin;
control->bmax = bmax;
control->dbeta = dbeta;
control->wmin = wmin;
control->wmax = wmax;
control->dw = dw;
control->fix_beta = fbeta;
control->fix_wmega = fwmega;
}
int main(int argc, char ** argv)
{
//Init Phantom
phantomOpen();
printf("Phantom Opened \n");
//Init ROS
ros::init(argc, argv , "phantom_slave");
ros::NodeHandle nh;
data= fopen("data.txt","w");
// Publisher
Mas2Sla_pub = nh.advertise<geometry_msgs::PointStamped>("/MasToSla",1);
Mas2SlaEnergy_pub = nh.advertise<geometry_msgs::PointStamped>("/MasToSlaEnergy",1);
//Subcriber
ros::Subscriber SlaToMasVs_sub = nh.subscribe<geometry_msgs::PointStamped>("/SlatoMasVs", 1,SlaToMasVsDelay);
ros::Subscriber SlaToMasFe_sub = nh.subscribe<geometry_msgs::PointStamped>("/SlatoMasFe", 1,SlaToMasFeDelay);
ros::Subscriber SlaToMasFv_sub = nh.subscribe<geometry_msgs::PointStamped>("/SlatoMasFv", 1,SlaToMasFvDelay);
//Asign callback
hPhantomMain = hdScheduleAsynchronous(phantom_callback, 0, HD_MAX_SCHEDULER_PRIORITY);
// hdSetSchedulerRate(1000);
init_Data();
//Start Phantom Scheduler
phantomRun();
//waiting
while(ros::ok() && nh.ok())
{
ros::spinOnce();
}
phantomClose();
return 1;
}
