749 lines
24 KiB
C++
749 lines
24 KiB
C++
#include <math.h>
|
|
#include <fstream>
|
|
#include <string.h>
|
|
#include "MatriceVectLib.h" //LSRO Matrix library (wayne erweitert)
|
|
|
|
|
|
//#define _USING_ROS //comment out when using not using ros.
|
|
|
|
#ifdef _USING_ROS
|
|
#include "ros/ros.h"
|
|
#include "command/setvalue.h"
|
|
#include "command/getvalue.h"
|
|
#include "command/newgon_state.h"
|
|
#include <sensor_msgs/JointState.h>
|
|
#endif
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
class AxisDef
|
|
{
|
|
public:
|
|
static const int NAMELENGTH = 8;
|
|
//AXIS PARAMETERS
|
|
char name[NAMELENGTH+1]; //axis name
|
|
char unit_usr[NAMELENGTH+1]; //user unit name (string)
|
|
char unit_ctr[NAMELENGTH+1]; //control unit name (string)
|
|
double scale = 1.; //
|
|
double offset = 0.; //UserUnits = (ControlUnits * scale) + offset
|
|
//All limits in User Units:
|
|
double LimNeg = 0.;
|
|
double LimPos = 0.;
|
|
double maxSpeed = 0.;
|
|
double minSpeed = 0.;
|
|
double maxJogSpeed = 0.;
|
|
double minJogSpeed = 0.;
|
|
//Nominal speeds:
|
|
double moveSpeed = 0.;
|
|
double jogSpeed = 0.;
|
|
//Motion paramters
|
|
double dt = 0.001; //time step
|
|
double vmax = 0.1; //maximum velocity
|
|
double amax = 0.1; //maximum acceleration
|
|
double xdb = 0.0005; //position loop deadband.
|
|
|
|
//AXIS STATUS:
|
|
int state_init = 0;
|
|
int state_motion = 0;
|
|
|
|
double t = 0; //current time
|
|
double x = 0; //current position
|
|
double v = 0; //current velocity
|
|
double a = 0; //current acceleration
|
|
|
|
double xT = 0; //target position
|
|
|
|
//Default Constructor:
|
|
AxisDef();
|
|
|
|
//Methods:
|
|
int setName(const char * newname);
|
|
int setUnitUsr(const char * newname);
|
|
int setUnitCtr(const char * newname);
|
|
int setStr(const char * setstring, char * targetstring);
|
|
int set_target(double target);
|
|
int step();
|
|
};
|
|
|
|
AxisDef::AxisDef()
|
|
{
|
|
//initialize strings to "" (to avoid crap upon creation)
|
|
setName("");
|
|
setUnitUsr("");
|
|
setUnitCtr("");
|
|
}
|
|
|
|
int AxisDef::setName(const char * newname)
|
|
{
|
|
return setStr(newname, name);
|
|
}
|
|
|
|
int AxisDef::setUnitUsr(const char * newname)
|
|
{
|
|
return setStr(newname, unit_usr);
|
|
}
|
|
|
|
int AxisDef::setUnitCtr(const char * newname)
|
|
{
|
|
return setStr(newname, unit_ctr);
|
|
}
|
|
|
|
int AxisDef::setStr(const char * setstring, char * targetstring)
|
|
// use: newgon.setName("Name")
|
|
// only the first NAMELENTH charaters will be used.
|
|
// if the given string is longer than NAMELENGTH, return 1
|
|
// if all goes well return 0
|
|
{
|
|
for (int i=0; i<NAMELENGTH+1; i++)
|
|
{
|
|
if (setstring[i] == 0) //if end of string of newname
|
|
{
|
|
targetstring[i]=setstring[i];
|
|
break;
|
|
}
|
|
else { //normal characters in newname
|
|
targetstring[i]=setstring[i];
|
|
}
|
|
if (i==NAMELENGTH) //if the end of name is reached, terminate with 0
|
|
{
|
|
targetstring[i]=0;
|
|
return 1; // return 1 means: string too long.
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int AxisDef::set_target(double target)
|
|
{
|
|
xT = target;
|
|
return 0;
|
|
}
|
|
|
|
int AxisDef::step()
|
|
{
|
|
t += dt; //increment by time step
|
|
double xrem = xT-x; //remaining path
|
|
if (fabs(xrem) < xdb) return 1; //if in deadband, don't do anythhing
|
|
|
|
double xsgn = xrem<0?-1:1; //sgn(xrem)
|
|
double bremsweg = v*v/2/amax; //bremsweg (based on current speed)
|
|
double asgn = fabs(xrem)<fabs(bremsweg)?-1:1; //if bremsweg too short deccelerate
|
|
a = xsgn*asgn*amax; //calculate new a
|
|
v = v + a*dt; //calculate new v
|
|
if (v> vmax) {v = vmax; a=0;} //bound v
|
|
if (v<-vmax) {v = -vmax; a=0;}
|
|
x = x + v*dt + 0.5*a*dt*dt; //calculate new x
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
class NewGon
|
|
{
|
|
public:
|
|
static const int naxes = 6; //define here the number of axes of NewGon
|
|
static const int namelength = 20; //define here the max string length of names
|
|
|
|
//NOTE: AXIS DEFINITIONS START AT 1 not 0, therefore axis[0] is empty and not used.
|
|
|
|
//Create all Axis objects
|
|
AxisDef axis[naxes+1];
|
|
|
|
//General Parameters
|
|
char name[namelength+1];
|
|
double dt = 0.001;
|
|
|
|
//GENERAL STATUS
|
|
int MODE = 0;
|
|
double timestamp = 0.;
|
|
|
|
//Geometrical Lengths
|
|
double l01 = 42.5e-3;
|
|
double l11 = 25e-3 - (17e-3)/2; //half distance between sliders table midline
|
|
double l12 = l11;
|
|
double l21 = l11;
|
|
double l22 = l11;
|
|
double l23 = 13.5e-3; //Distance between q1 & q2 stage level
|
|
double l31 = 11.5e-3; //Distance from q3 table to middle of red part
|
|
double l32 = 68.5e-3 - (80e-3)/2;
|
|
double l33 = l31;
|
|
double l34 = l32;
|
|
double l41 = 76.5e-3;
|
|
double l42 = 25.5e-3;
|
|
double l51 = 10e-3;
|
|
double l52 = 2.5e-3;
|
|
double l61 = 64.422e-3; // Connecting rod length
|
|
double l71 = 5e-3; // Swing dimensions
|
|
double l72 = 17.67e-3;
|
|
double l73 = 5.2e-3;
|
|
double l74 = 1.53e-3;
|
|
|
|
double s1, s2, s3, s4; //Slider Motor Coordinates
|
|
double omega_pos, phi_pos; //Omega & Phi Motor Coordinates
|
|
|
|
double SHx, SHy, SHz; //Sample Holder Vector
|
|
double Ox, Oy, Oz; //Sample Centre Position (0 Ref: centre of aerotech tabletop)
|
|
|
|
|
|
//Constructors:
|
|
NewGon();
|
|
|
|
//Methods:
|
|
int setName(const char * newname);
|
|
int setRate(double frequency);
|
|
int step();
|
|
#ifdef _USING_ROS
|
|
bool setValueROS(command::setvalue::Request& req,
|
|
command::setvalue::Response& res);
|
|
bool getValueROS(command::getvalue::Request& req,
|
|
command::getvalue::Response& res);
|
|
int publishROS(ros::Publisher * pPub);
|
|
#endif
|
|
|
|
int IK(double *Point_op, double *Point_art);
|
|
int FK(double* Point_art, double* Point_op);
|
|
int readParamFile(const char* filename);
|
|
int writeParamFile(const char* filename);
|
|
int printParams();
|
|
int printStatus();
|
|
};
|
|
|
|
NewGon::NewGon()
|
|
{
|
|
|
|
}
|
|
|
|
int NewGon::setName(const char * newname)
|
|
// use: newgon.setName("Name")
|
|
// only the first NAMELENTH charaters will be used.
|
|
// if the given string is longer than NAMELENGTH, return 1
|
|
// if all goes well return 0
|
|
{
|
|
for (int i=0; i<namelength+1; i++)
|
|
{
|
|
if (newname[i] == 0) //if end of string of newname
|
|
{
|
|
name[i]=newname[i];
|
|
break;
|
|
}
|
|
else { //normal characters in newname
|
|
name[i]=newname[i];
|
|
}
|
|
if (i==namelength) //if the end of name is reached, terminate with 0
|
|
{
|
|
name[i]=0;
|
|
return 1; // return 1 means: string too long.
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int NewGon::setRate(double frequency)
|
|
{
|
|
dt = 1./frequency;
|
|
//Watch out frequency should be < 0
|
|
return 0;
|
|
}
|
|
|
|
int NewGon::step ()
|
|
{
|
|
for (int i=1; i<naxes+1; i++)
|
|
{
|
|
axis[i].dt = dt;
|
|
axis[i].step();
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
//SET COMMAND LIST:
|
|
#ifdef _USING_ROS
|
|
bool NewGon::setValueROS(command::setvalue::Request &req,
|
|
command::setvalue::Response &res)
|
|
{
|
|
ROS_INFO("setvalue request : %s: %lf", req.axis.c_str(), req.value);
|
|
if (!req.axis.compare("s1"))
|
|
{
|
|
axis[1].xT = req.value;
|
|
res.response = "Axis 1: " + std::to_string(req.value);
|
|
}
|
|
else if (!req.axis.compare("s2"))
|
|
{
|
|
axis[2].xT = req.value;
|
|
res.response = "Axis 2: " + std::to_string(req.value);
|
|
}
|
|
else if (!req.axis.compare("s3"))
|
|
{
|
|
axis[3].xT = req.value;
|
|
res.response = "Axis 3: " + std::to_string(req.value);
|
|
}
|
|
else if (!req.axis.compare("s4"))
|
|
{
|
|
axis[4].xT = req.value;
|
|
res.response = "Axis 4: " + std::to_string(req.value);
|
|
}
|
|
else if (!req.axis.compare("s5"))
|
|
{
|
|
axis[4].xT = req.value;
|
|
res.response = "Axis 5: " + std::to_string(req.value);
|
|
}
|
|
else if (!req.axis.compare("MODE"))
|
|
{
|
|
MODE = (int)req.value;
|
|
res.response = "MODE: " + std::to_string(req.value);
|
|
}
|
|
|
|
|
|
ROS_INFO("setvalue response: %s", res.response.c_str());
|
|
return true;
|
|
}
|
|
|
|
bool NewGon::getValueROS(command::getvalue::Request &req,
|
|
command::getvalue::Response &res)
|
|
{
|
|
ROS_INFO("getvalue request : %s", req.axis.c_str());
|
|
res.response = "ok.";
|
|
ROS_INFO("getvalue response: %s", res.response.c_str());
|
|
return true;
|
|
}
|
|
|
|
int NewGon::publishROS(ros::Publisher * pPub)
|
|
{
|
|
/* command::newgon_state msg;
|
|
msg.s1 = axis[1].x;
|
|
msg.s2 = axis[2].x;
|
|
msg.s3 = axis[3].x;
|
|
msg.s4 = axis[4].x;
|
|
msg.s5 = axis[5].x;
|
|
|
|
pPub->publish(msg);
|
|
*/
|
|
sensor_msgs::JointState joint_state;
|
|
|
|
joint_state.header.stamp = ros::Time::now();
|
|
joint_state.name.resize(6);
|
|
joint_state.position.resize(6);
|
|
joint_state.name[0] ="axisomega";
|
|
joint_state.position[0] = axis[1].x;
|
|
joint_state.name[1] ="axis1";
|
|
joint_state.position[1] = axis[2].x;
|
|
joint_state.name[2] ="axis2";
|
|
joint_state.position[2] = axis[3].x;
|
|
joint_state.name[3] ="axis3";
|
|
joint_state.position[3] = axis[4].x;
|
|
joint_state.name[4] ="axistheta";
|
|
joint_state.position[4] = axis[5].x;
|
|
joint_state.name[5] ="axisphi";
|
|
joint_state.position[5] = axis[6].x;
|
|
pPub->publish(joint_state);
|
|
|
|
}
|
|
#endif
|
|
int NewGon::IK(double *Point_op, double *Point_art)
|
|
{
|
|
//Assign Operational Coordinates
|
|
double SHx = Point_op[0]; //sample holder vector
|
|
double SHy = Point_op[1];
|
|
double SHz = Point_op[2];
|
|
double CHI = Point_op[3]; //UPPER CASE angles are in DEG (lower case are in rad)
|
|
double PHI = Point_op[4];
|
|
double OMEGA = Point_op[5];
|
|
|
|
double Ox = Point_op[6]; //Centre of Interest (described with Frame 0)
|
|
double Oy = Point_op[7];
|
|
double Oz = Point_op[8];
|
|
|
|
|
|
//convert to radians (note: angles in lowercase are used later on)
|
|
// double chi = CHI/360*2*M_PI;
|
|
// double omega = OMEGA/360*2*M_PI;
|
|
// double phi = PHI/360*2*M_PI;
|
|
double chi = CHI;
|
|
double omega = OMEGA;
|
|
double phi = PHI;
|
|
//set initial value for theta
|
|
double theta = chi;
|
|
|
|
//Set Target Coordinates
|
|
double xt[6] = {Ox,Oy,Oz,omega,chi,phi};
|
|
//Motor Start Values (q1,q2,q3,theta,q5,q6)
|
|
double qc[6] = {1e-3,1e-3,0,0,0,0}; //qc are current q values
|
|
double xc[6]; //xc are current x values
|
|
double deltax[6];
|
|
double deltaq[6];
|
|
double J[6][6];
|
|
int pivot[6];
|
|
|
|
bool loopcond = true;
|
|
int loopcounter = 0;
|
|
int maxloops=30;
|
|
|
|
while (loopcond) {
|
|
//Calculate current x values based on current q:
|
|
xc[0] = l01 + l23 + l32 + l41 + qc[2] + SHx*cos(qc[3]) + l74*cos(qc[3]) + l71*sin(qc[3])
|
|
- SHy*cos(qc[4])*sin(qc[3]) + SHz*sin(qc[4])*sin(qc[3]);
|
|
xc[1] = l31*cos(qc[5]) - SHz*(cos(qc[4])*sin(qc[5]) + cos(qc[5])*cos(qc[3])*sin(qc[4]))
|
|
- SHy*(sin(qc[4])*sin(qc[5]) - cos(qc[4])*cos(qc[5])*cos(qc[3])) - l42*cos(qc[5])
|
|
+ qc[0]*cos(qc[5]) - qc[1]*sin(qc[5]) + SHx*cos(qc[5])*sin(qc[3])
|
|
- l71*cos(qc[5])*cos(qc[3]) + l74*cos(qc[5])*sin(qc[3]);
|
|
xc[2] = SHy*(cos(qc[5])*sin(qc[4]) + cos(qc[4])*cos(qc[3])*sin(qc[5]))
|
|
+ SHz*(cos(qc[4])*cos(qc[5]) - cos(qc[3])*sin(qc[4])*sin(qc[5]))
|
|
+ qc[1]*cos(qc[5]) + l31*sin(qc[5]) - l42*sin(qc[5]) + qc[0]*sin(qc[5])
|
|
+ SHx*sin(qc[5])*sin(qc[3]) - l71*cos(qc[3])*sin(qc[5])
|
|
+ l74*sin(qc[5])*sin(qc[3]);
|
|
xc[3] = (180*qc[5])/M_PI;
|
|
xc[4] = (180*qc[3])/M_PI;
|
|
xc[5] = (180*qc[4])/M_PI;
|
|
|
|
//Calculate the Jacobian:
|
|
//first row:
|
|
J[0][0] = 0; J[0][1] = 0; J[0][2] = 1;
|
|
J[0][3] = l71*cos(qc[3]) - SHx*sin(qc[3]) - l74*sin(qc[3]) - SHy*cos(qc[4])*cos(qc[3])
|
|
+ SHz*cos(qc[3])*sin(qc[4]);
|
|
J[0][4] = SHz*cos(qc[4])*sin(qc[3]) + SHy*sin(qc[4])*sin(qc[3]);
|
|
J[0][5]= 0;
|
|
//second row:
|
|
J[1][0] = cos(qc[5]); J[1][1] = -sin(qc[5]); J[1][2]= 0;
|
|
J[1][3] = SHx*cos(qc[5])*cos(qc[3]) + l74*cos(qc[5])*cos(qc[3]) + l71*cos(qc[5])*sin(qc[3])
|
|
- SHy*cos(qc[4])*cos(qc[5])*sin(qc[3]) + SHz*cos(qc[5])*sin(qc[4])*sin(qc[3]);
|
|
J[1][4] = SHz*(sin(qc[4])*sin(qc[5]) - cos(qc[4])*cos(qc[5])*cos(qc[3]))
|
|
- SHy*(cos(qc[4])*sin(qc[5]) + cos(qc[5])*cos(qc[3])*sin(qc[4]));
|
|
J[1][5] = l42*sin(qc[5]) - SHz*(cos(qc[4])*cos(qc[5]) - cos(qc[3])*sin(qc[4])*sin(qc[5]))
|
|
- qc[1]*cos(qc[5]) - l31*sin(qc[5]) - SHy*(cos(qc[5])*sin(qc[4])
|
|
+ cos(qc[4])*cos(qc[3])*sin(qc[5])) - qc[0]*sin(qc[5]) - SHx*sin(qc[5])*sin(qc[3])
|
|
+ l71*cos(qc[3])*sin(qc[5]) - l74*sin(qc[5])*sin(qc[3]);
|
|
//third row:
|
|
J[2][0] = sin(qc[5]); J[2][1]= cos(qc[5]); J[2][2]= 0;
|
|
J[2][3] = SHx*cos(qc[3])*sin(qc[5]) + l74*cos(qc[3])*sin(qc[5]) + l71*sin(qc[5])*sin(qc[3])
|
|
- SHy*cos(qc[4])*sin(qc[5])*sin(qc[3]) + SHz*sin(qc[4])*sin(qc[5])*sin(qc[3]);
|
|
J[2][4] = SHy*(cos(qc[4])*cos(qc[5]) - cos(qc[3])*sin(qc[4])*sin(qc[5]))
|
|
- SHz*(cos(qc[5])*sin(qc[4]) + cos(qc[4])*cos(qc[3])*sin(qc[5]));
|
|
J[2][5] = l31*cos(qc[5]) - SHz*(cos(qc[4])*sin(qc[5]) + cos(qc[5])*cos(qc[3])*sin(qc[4]))
|
|
- SHy*(sin(qc[4])*sin(qc[5]) - cos(qc[4])*cos(qc[5])*cos(qc[3]))
|
|
- l42*cos(qc[5]) + qc[0]*cos(qc[5]) - qc[1]*sin(qc[5]) + SHx*cos(qc[5])*sin(qc[3])
|
|
- l71*cos(qc[5])*cos(qc[3]) + l74*cos(qc[5])*sin(qc[3]);
|
|
//fourth row:
|
|
J[3][0] = 0; J[3][1]= 0; J[3][2]= 0; J[3][3]= 0;J[3][4]= 0;J[3][5]= 180/M_PI;
|
|
//fifth row:
|
|
J[4][0] = 0; J[4][1]= 0; J[4][2]= 0; J[4][3]= 180/M_PI; J[4][4]= 0;J[4][5] =0;
|
|
//sixth row:
|
|
J[5][0] = 0; J[5][1]= 0; J[5][2]= 0; J[5][3]= 0; J[5][4]=180/M_PI; J[5][5]=0;
|
|
|
|
|
|
//Calculate x error (target - current)
|
|
float deltax_abs = 0;
|
|
for (int i=0; i<6; i++) {
|
|
deltax[i]=xt[i]-xc[i];
|
|
deltax_abs += deltax[i]*deltax[i];
|
|
}
|
|
|
|
//If abs error is small enough, get out of loop.
|
|
if (sqrt(deltax_abs)<1e-9) {
|
|
loopcond = false;
|
|
printf("solution found\n");
|
|
}
|
|
|
|
//Solve equation system (LU Decomposition, LU Solve)
|
|
int decompOK = 0, solveOK = 0;
|
|
decompOK= Doolittle_LU_Decomposition_with_Pivoting(&J[0][0], pivot, 6);
|
|
//solveOK = Doolittle_LU_with_Pivoting_Solve(&J[0][0], b, pivot, x, 6);
|
|
solveOK = Doolittle_LU_with_Pivoting_Solve(&J[0][0], deltax, pivot, deltaq, 6);
|
|
|
|
//Calculate qc += deltaq
|
|
for (int i=0; i<6; i++)
|
|
qc[i] += deltaq[i];
|
|
|
|
|
|
loopcounter++;
|
|
if (loopcounter > maxloops){
|
|
loopcond = false;
|
|
printf("%d Iterations reached.\n",maxloops);
|
|
}
|
|
}
|
|
//export parameters
|
|
Point_art[0] = qc[0];
|
|
Point_art[1] = qc[1];
|
|
Point_art[2] = qc[2];
|
|
// Point_art[3] = qc[3];
|
|
Point_art[4] = qc[4];
|
|
Point_art[5] = qc[5];
|
|
|
|
|
|
// Calculate q4 based on q3 and theta
|
|
// Use the newton method:
|
|
// Starting value theta_0:
|
|
double q4 = 0;
|
|
theta = qc[3];
|
|
// Set loop conditions
|
|
loopcond = true;
|
|
loopcounter = 0;
|
|
maxloops = 30;
|
|
while (loopcond) {
|
|
double f, f_diff;
|
|
//Newton Formula, using f, and f_diff,
|
|
f = sqrt((l32 - l34 + l41 - l51 + qc[2] - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))*
|
|
(l32 - l34 + l41 - l51 + qc[2] - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))
|
|
+ (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))*
|
|
(l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))) - l61;
|
|
f_diff = -(2*l32 - 2*l34 + 2*l41 - 2*l51 + 2*qc[2] - 2*q4 + 2*sin(theta)*(l71 + l73)
|
|
- 2*l72*cos(theta))
|
|
/(2* sqrt((l32 - l34 + l41 - l51 + qc[2] - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))*
|
|
(l32 - l34 + l41 - l51 + qc[2] - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))
|
|
+ (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))*
|
|
(l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))));
|
|
q4 = q4 - (f/f_diff);
|
|
//Increase loop counter, and stop looping if maxloop is reached
|
|
loopcounter++;
|
|
if (loopcounter> maxloops) {
|
|
loopcond = false;
|
|
printf("%d iterations reached. No solution for q4 found.\n", maxloops);
|
|
}
|
|
//Calculate residual error, and if sufficiently small stop looping
|
|
if (fabs(f/f_diff)<1e-9){
|
|
loopcond = false;
|
|
}
|
|
}
|
|
|
|
Point_art[3] = q4;
|
|
|
|
printf("ok\n");
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
int NewGon::readParamFile(const char * filename)
|
|
{
|
|
std::string word;
|
|
double value;
|
|
|
|
int count = 0;
|
|
std::ifstream infile(filename);
|
|
while (infile >> word >> value ) {
|
|
std::cout << "word: " << word << std::endl;
|
|
std::cout << "value:" << value<< std::endl;
|
|
if (!word.compare("dt")){
|
|
dt = value;
|
|
std::cout << "recognized:" << word <<":"<< value<< std::endl;
|
|
count++;
|
|
}
|
|
else if (!word.compare("dt")) {
|
|
|
|
} else {
|
|
|
|
}
|
|
|
|
}
|
|
return count;
|
|
}
|
|
*/
|
|
|
|
int NewGon::readParamFile(const char* filename)
|
|
{
|
|
//define variables
|
|
FILE * pFile;
|
|
char mode[4]="r"; //mode read
|
|
char read_str[1000]; //string buffer
|
|
double read_double = 0.; //double buffer
|
|
//open file:
|
|
pFile = fopen(filename, mode);
|
|
//check if file can be opened:
|
|
if (pFile==NULL){
|
|
return 1; //error reading file
|
|
} else {
|
|
//start reading line by line:
|
|
while (!feof(pFile)) {
|
|
//fgets(linebuffer, 100, pFile); //read a line in.
|
|
fscanf (pFile, "%s\t%lf\n", read_str, &read_double);
|
|
if (strcmp(read_str,"SHx")==0){ SHx=read_double;printf("SHx=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"SHy")==0){ SHy=read_double;printf("SHy=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"SHz")==0){ SHz=read_double;printf("SHz=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"Ox")==0){ Ox=read_double;printf("Ox=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"Oy")==0){ Oy=read_double;printf("Oy=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"Oz")==0){ Oz=read_double;printf("Oz=%lf\n",read_double);}
|
|
|
|
// else if (strcmp(read_str,"offsetR1")==0){ offsetR1=read_double;printf("offsetR1=%lf\n",read_double);}
|
|
// else if (strcmp(read_str,"offsetR2")==0){ offsetR2=read_double;printf("offsetR2=%lf\n",read_double);}
|
|
// else if (strcmp(read_str,"offsetR3")==0){ offsetR3=read_double;printf("offsetR3=%lf\n",read_double);}
|
|
// else if (strcmp(read_str,"offsetR4")==0){ offsetR4=read_double;printf("offsetR4=%lf\n",read_double);}
|
|
//
|
|
// else if (strcmp(read_str,"offsetS1")==0){ offsetS1=read_double;printf("offsetS1=%lf\n",read_double);}
|
|
// else if (strcmp(read_str,"offsetS2")==0){ offsetS2=read_double;printf("offsetS2=%lf\n",read_double);}
|
|
// else if (strcmp(read_str,"offsetS3")==0){ offsetS3=read_double;printf("offsetS3=%lf\n",read_double);}
|
|
// else if (strcmp(read_str,"offsetS4")==0){ offsetS4=read_double;printf("offsetS4=%lf\n",read_double);}
|
|
|
|
else if (strcmp(read_str,"l01")==0){ l01=read_double;printf("l01=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l11")==0){ l11=read_double;printf("l11=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l12")==0){ l12=read_double;printf("l12=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l21")==0){ l21=read_double;printf("l21=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l22")==0){ l22=read_double;printf("l22=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l23")==0){ l23=read_double;printf("l23=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l31")==0){ l31=read_double;printf("l31=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l32")==0){ l32=read_double;printf("l32=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l33")==0){ l33=read_double;printf("l33=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l34")==0){ l34=read_double;printf("l34=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l41")==0){ l41=read_double;printf("l41=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l42")==0){ l42=read_double;printf("l42=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l51")==0){ l51=read_double;printf("l51=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l52")==0){ l52=read_double;printf("l52=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l61")==0){ l61=read_double;printf("l61=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l71")==0){ l71=read_double;printf("l71=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l72")==0){ l72=read_double;printf("l72=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l73")==0){ l73=read_double;printf("l73=%lf\n",read_double);}
|
|
else if (strcmp(read_str,"l74")==0){ l74=read_double;printf("l74=%lf\n",read_double);}
|
|
}
|
|
fclose(pFile);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int NewGon::writeParamFile(const char* filename)
|
|
{
|
|
//define variables
|
|
FILE * pFile;
|
|
char mode[4]="w"; //mode write
|
|
//open file:
|
|
pFile = fopen(filename, mode);
|
|
//check if file can be opened:
|
|
if (pFile==NULL){
|
|
return 1; //error reading file
|
|
} else {
|
|
//start writing line by line:
|
|
fprintf(pFile, "SHx\t%lf\n",SHx);
|
|
fprintf(pFile, "SHy\t%lf\n",SHy);
|
|
fprintf(pFile, "SHz\t%lf\n",SHz);
|
|
fprintf(pFile, "Ox\t%lf\n",Ox);
|
|
fprintf(pFile, "Oy\t%lf\n",Oy);
|
|
fprintf(pFile, "Oz\t%lf\n",Oz);
|
|
|
|
// fprintf(pFile, "offsetR1\t%lf\n",offsetR1);
|
|
// fprintf(pFile, "offsetR2\t%lf\n",offsetR2);
|
|
// fprintf(pFile, "offsetR3\t%lf\n",offsetR3);
|
|
// fprintf(pFile, "offsetR4\t%lf\n",offsetR4);
|
|
//
|
|
// fprintf(pFile, "offsetS1\t%lf\n",offsetS1);
|
|
// fprintf(pFile, "offsetS2\t%lf\n",offsetS2);
|
|
// fprintf(pFile, "offsetS3\t%lf\n",offsetS3);
|
|
// fprintf(pFile, "offsetS4\t%lf\n",offsetS4);
|
|
|
|
fprintf(pFile, "l01\t%lf\n",l01);
|
|
fprintf(pFile, "l11\t%lf\n",l11);
|
|
fprintf(pFile, "l12\t%lf\n",l12);
|
|
fprintf(pFile, "l21\t%lf\n",l21);
|
|
fprintf(pFile, "l22\t%lf\n",l22);
|
|
fprintf(pFile, "l23\t%lf\n",l23);
|
|
fprintf(pFile, "l31\t%lf\n",l31);
|
|
fprintf(pFile, "l32\t%lf\n",l32);
|
|
fprintf(pFile, "l33\t%lf\n",l33);
|
|
fprintf(pFile, "l34\t%lf\n",l34);
|
|
fprintf(pFile, "l41\t%lf\n",l41);
|
|
fprintf(pFile, "l42\t%lf\n",l42);
|
|
fprintf(pFile, "l51\t%lf\n",l51);
|
|
fprintf(pFile, "l52\t%lf\n",l52);
|
|
fprintf(pFile, "l61\t%lf\n",l61);
|
|
fprintf(pFile, "l71\t%lf\n",l71);
|
|
fprintf(pFile, "l72\t%lf\n",l72);
|
|
fprintf(pFile, "l73\t%lf\n",l73);
|
|
fprintf(pFile, "l74\t%lf\n",l74);
|
|
|
|
fclose(pFile);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int NewGon::printParams()
|
|
{
|
|
//start writing line by line:
|
|
// printf("SHx\t%lf\n",SHx);
|
|
// printf("SHy\t%lf\n",SHy);
|
|
// printf("SHz\t%lf\n",SHz);
|
|
// printf("Ox\t%lf\n",Ox);
|
|
// printf("Oy\t%lf\n",Oy);
|
|
// printf("Oz\t%lf\n",Oz);
|
|
|
|
// printf("offsetR1\t%lf\n",offsetR1);
|
|
// printf("offsetR2\t%lf\n",offsetR2);
|
|
// printf("offsetR3\t%lf\n",offsetR3);
|
|
// printf("offsetR4\t%lf\n",offsetR4);
|
|
//
|
|
// printf("offsetS1\t%lf\n",offsetS1);
|
|
// printf("offsetS2\t%lf\n",offsetS2);
|
|
// printf("offsetS3\t%lf\n",offsetS3);
|
|
// printf("offsetS4\t%lf\n",offsetS4);
|
|
|
|
printf("l01\t%lf [mm]\n",l01*1000);
|
|
printf("l11\t%lf [mm]\n",l11*1000);
|
|
printf("l12\t%lf [mm]\n",l12*1000);
|
|
printf("l21\t%lf [mm]\n",l21*1000);
|
|
printf("l22\t%lf [mm]\n",l22*1000);
|
|
printf("l23\t%lf [mm]\n",l23*1000);
|
|
printf("l31\t%lf [mm]\n",l31*1000);
|
|
printf("l32\t%lf [mm]\n",l32*1000);
|
|
printf("l33\t%lf [mm]\n",l33*1000);
|
|
printf("l34\t%lf [mm]\n",l34*1000);
|
|
printf("l41\t%lf [mm]\n",l41*1000);
|
|
printf("l42\t%lf [mm]\n",l42*1000);
|
|
printf("l51\t%lf [mm]\n",l51*1000);
|
|
printf("l52\t%lf [mm]\n",l52*1000);
|
|
printf("l61\t%lf [mm]\n",l61*1000);
|
|
printf("l71\t%lf [mm]\n",l71*1000);
|
|
printf("l72\t%lf [mm]\n",l72*1000);
|
|
printf("l73\t%lf [mm]\n",l73*1000);
|
|
printf("l74\t%lf [mm]\n",l74*1000);
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
int NewGon::printStatus() {
|
|
|
|
printf("MODE\t%d\n", MODE);
|
|
printf("SHx\t%lf\n",SHx);
|
|
printf("SHy\t%lf\n",SHy);
|
|
printf("SHz\t%lf\n",SHz);
|
|
printf("CHI\t%lf\n",CHI);
|
|
printf("PHI\t%lf\n",PHI);
|
|
printf("OMEGA\t%lf\n",OMEGA);
|
|
printf("Ox\t%lf\n",Ox);
|
|
printf("Oy\t%lf\n",Oy);
|
|
printf("Oz\t%lf\n",Oz);
|
|
printf("s1\t%lf\n",s1);
|
|
printf("s2\t%lf\n",s2);
|
|
printf("s3\t%lf\n",s3);
|
|
printf("s4\t%lf\n",s4);
|
|
printf("phimotor\t%lf\n",phimotor);
|
|
printf("omegamotor\t%lf\n",omegamotor);
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|