smargopolo/paramfile/readparamfile.h

#include <math.h>

class Trajectory
{
    public:
    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
    double xdb=0.0001; //position loop deadband.
 
    //parameters
    double vmax = 0;  //maximum velocity
    double amax = 0;  //maximum acceleration
  
    double dt = 0.001; //time step

    int set_target(double target);
    int runstep ();
    private:

};


int Trajectory::set_target(double target)
{
    xT = target;
    return 0;
}

int Trajectory::runstep()
{
    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 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 limit 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.;


    Trajectory traj;
    //AXIS STATUS:
    double timestamp = 0.;
    int state_init = 0;
    int state_motion = 0;
    double pos = 0.;
    double v = 0.;
    double a = 0.;
 
    double targetpos = 0.;

    //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);
};

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;
}





////////////////////////////////////////////////////////////////
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
    
    //Create all Axis objects
    AxisDef axis[naxes+1];

    //General Parameters
    char name[namelength+1];

    //GENERAL STATUS
    int MODE = 0.;
    double timestamp = 0.;

    //Constructors:
    NewGon() { }

    //Methods:
    int setName(const char * newname);
    int runstep();
};

//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::runstep()
{
    return 0;
}