smargopolo/simultest/Test.cpp

#include <iostream>
#include <chrono>
#include <ctime>
#include "NewGon.h"
#include "Motion.h"

int initialize (NewGon * myGon) 
{
    //get ready to loop
    myGon->setName("WayneGon");

    myGon->axis[0].setName("Axis_1");
    myGon->axis[0].setUnitUsr("mm");
    myGon->axis[0].setUnitCtr("nm");
    myGon->axis[0].scale = 1;
    myGon->axis[0].offset = 0;
    myGon->axis[0].LimNeg = -2.;
    myGon->axis[0].LimPos =  2.;   
    myGon->axis[0].maxSpeed = 100.;
    myGon->axis[0].minSpeed = 0.;
    myGon->axis[0].maxJogSpeed = 100.;
    myGon->axis[0].minJogSpeed = 0.;
    myGon->axis[0].moveSpeed = 10.;
    myGon->axis[0].jogSpeed = 10.;

    myGon->axis[1].setName("Axis_2");
    myGon->axis[1].setUnitUsr("mm");
    myGon->axis[1].setUnitCtr("nm");
    myGon->axis[1].scale = 1;
    myGon->axis[1].offset = 0;
    myGon->axis[1].LimNeg = -2.;
    myGon->axis[1].LimPos =  2.;   
    myGon->axis[1].maxSpeed = 100.;
    myGon->axis[1].minSpeed = 0.;
    myGon->axis[1].maxJogSpeed = 100.;
    myGon->axis[1].minJogSpeed = 0.;
    myGon->axis[1].moveSpeed = 10.;
    myGon->axis[1].jogSpeed = 10.;

    myGon->axis[2].setName("Axis_3");
    myGon->axis[2].setUnitUsr("mm");
    myGon->axis[2].setUnitCtr("nm");
    myGon->axis[2].scale = 1;
    myGon->axis[2].offset = 0;
    myGon->axis[2].LimNeg = -2.;
    myGon->axis[2].LimPos =  2.;   
    myGon->axis[2].maxSpeed = 100.;
    myGon->axis[2].minSpeed = 0.;
    myGon->axis[2].maxJogSpeed = 100.;
    myGon->axis[2].minJogSpeed = 0.;
    myGon->axis[2].moveSpeed = 10.;
    myGon->axis[2].jogSpeed = 10.;

    myGon->axis[3].setName("Axis_4");
    myGon->axis[3].setUnitUsr("mm");
    myGon->axis[3].setUnitCtr("nm");
    myGon->axis[3].scale = 1;
    myGon->axis[3].offset = 0;
    myGon->axis[3].LimNeg = -2.;
    myGon->axis[3].LimPos =  2.;   
    myGon->axis[3].maxSpeed = 100.;
    myGon->axis[3].minSpeed = 0.;
    myGon->axis[3].maxJogSpeed = 100.;
    myGon->axis[3].minJogSpeed = 0.;
    myGon->axis[3].moveSpeed = 10.;
    myGon->axis[3].jogSpeed = 10.;

    myGon->axis[4].setName("Axis_5");
    myGon->axis[4].setUnitUsr("mm");
    myGon->axis[4].setUnitCtr("nm");
    myGon->axis[4].scale = 1;
    myGon->axis[4].offset = 0;
    myGon->axis[4].LimNeg = -2.;
    myGon->axis[4].LimPos =  2.;   
    myGon->axis[4].maxSpeed = 100.;
    myGon->axis[4].minSpeed = 0.;
    myGon->axis[4].maxJogSpeed = 100.;
    myGon->axis[4].minJogSpeed = 0.;
    myGon->axis[4].moveSpeed = 10.;
    myGon->axis[4].jogSpeed = 10.;

    myGon->axis[5].setName("Axis_6");
    myGon->axis[5].setUnitUsr("mm");
    myGon->axis[5].setUnitCtr("nm");
    myGon->axis[5].scale = 1;
    myGon->axis[5].offset = 0;
    myGon->axis[5].LimNeg = -2.;
    myGon->axis[5].LimPos =  2.;   
    myGon->axis[5].maxSpeed = 100.;
    myGon->axis[5].minSpeed = 0.;
    myGon->axis[5].maxJogSpeed = 100.;
    myGon->axis[5].minJogSpeed = 0.;
    myGon->axis[5].moveSpeed = 10.;
    myGon->axis[5].jogSpeed = 10.;

    return 0;
}

int mainloop (NewGon * myGon)
{
    float t_loop = 1000*1000; //in ns
    float elapsed_time;
    float last_time = 0;
    struct timespec req = {0};
    std::chrono::time_point<std::chrono::high_resolution_clock> start, end;

    float n = 0;
    float mean = 0.;
    float M2 = 0.0;
    float delta = 0.;

    int loopok = 1;
    while (loopok) {
        start = std::chrono::high_resolution_clock::now(); //chrono start

        std::cout << "Last Mainloop duration in ns:" << last_time << "\n";

        n += 1;
        delta = last_time - mean;
        mean += delta/n;
        M2+= delta*(last_time - mean);

        loopok++;
        if (loopok > 100) loopok = 0;
        
        end = std::chrono::high_resolution_clock::now(); //chrono stop
        elapsed_time = std::chrono::duration_cast<std::chrono::nanoseconds>(end-start).count();
        req.tv_nsec = t_loop - elapsed_time;
        nanosleep(&req, (struct timespec *)NULL);

        end = std::chrono::high_resolution_clock::now(); //chrono stop
        last_time = std::chrono::duration_cast<std::chrono::nanoseconds>(end-start).count();
    }

    std::cout << "Mean:" << mean << " Variance:" << M2/(n-1) << "\n";
//    float remaining_time = t_loop - elapsed_time;
//    struct timespec req = {0};
//    req.tv_nsec = remaining_time;
//    nanosleep(&req, (struct timespec *)NULL);

    return 0;
}


int finalize (NewGon * myGon)
{

    return 0;
}





int main ()
{
    std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
    start = std::chrono::high_resolution_clock::now(); //chrono start
    NewGon myGon; //Create Goniometer with 6 physical axes.
    initialize(&myGon);
    end = std::chrono::high_resolution_clock::now(); //chrono stop
    myGon.print();
    Motion motion1(&myGon.axis[0]); //Create motion object for axis 1 linking to axis[0]
    Motion motion2(&myGon.axis[1]); //Create motion object for axis 2
    Motion motion3(&myGon.axis[2]); //Create motion object for axis 3
    Motion motion4(&myGon.axis[3]); //Create motion object for axis 4
    Motion motion5(&myGon.axis[4]); //Create motion object for axis 5
    Motion motion6(&myGon.axis[5]); //Create motion object for axis 6

    mainloop(&myGon);



 
    int elapsed_time = std::chrono::duration_cast<std::chrono::microseconds>
                             (end-start).count();
    std::time_t end_time = std::chrono::system_clock::to_time_t(end);
 
    std::cout << "finished computation at " << std::ctime(&end_time)
              << "elapsed time: " << elapsed_time << "us\n";
    return 0;
}