227 lines
6.8 KiB
C++
227 lines
6.8 KiB
C++
#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;
|
||
}
|
||
|
||
#define NSEC_PER_SEC (1000000000) /* The number of nsecs per sec. */
|
||
static inline void tsnorm(struct timespec *ts)
|
||
{
|
||
while (ts->tv_nsec >= NSEC_PER_SEC) {
|
||
ts->tv_nsec -= NSEC_PER_SEC;
|
||
ts->tv_sec++;
|
||
}
|
||
}
|
||
|
||
|
||
/* Subtract the ‘struct timeval’ values X and Y, (X-Y)
|
||
Return 1 if the difference is negative, otherwise 0. */
|
||
int timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *y)
|
||
{
|
||
/* Perform the carry for the later subtraction by updating y. */
|
||
if (x->tv_nsec < y->tv_nsec) {
|
||
int n_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
|
||
y->tv_nsec -= 1000000000 * n_sec;
|
||
y->tv_sec += n_sec;
|
||
}
|
||
if (x->tv_nsec - y->tv_nsec > 1000000000) {
|
||
int n_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
|
||
y->tv_nsec += 1000000000 * n_sec;
|
||
y->tv_sec -= n_sec;
|
||
}
|
||
|
||
/* Compute the time remaining to wait.
|
||
tv_usec is certainly positive. */
|
||
result->tv_sec = x->tv_sec - y->tv_sec;
|
||
result->tv_nsec = x->tv_nsec - y->tv_nsec;
|
||
|
||
/* Return 1 if result is negative. */
|
||
return x->tv_sec < y->tv_sec;
|
||
}
|
||
|
||
|
||
|
||
|
||
int mainloop (NewGon * myGon)
|
||
{
|
||
//Variables for Loop Timing
|
||
float period = 1000*1000; //in ns (1ms)
|
||
static struct timespec t;
|
||
long int loopcount=0;
|
||
|
||
//Chronometry/statistics
|
||
struct timespec t_toc, t_lasttoc, t_duration;
|
||
double us_err, us_err_min=0, us_err_max=0, us_err_avg=0, us_err_rms=0;
|
||
|
||
//Setup Loop Conditions
|
||
int loopok = 1;
|
||
clock_gettime(CLOCK_MONOTONIC, &t); //set t to current time
|
||
|
||
while (loopok) {
|
||
loopcount++;
|
||
//Chronometry
|
||
t_lasttoc = t_toc;
|
||
clock_gettime(CLOCK_MONOTONIC, &t_toc); //TOC
|
||
timespec_subtract (&t_duration, &t_toc, &t_lasttoc); //get duration
|
||
us_err = ((t_duration.tv_nsec-period)/1000.); //in us
|
||
if (loopcount > 2) { //do statistics, omit first 2 values
|
||
if (us_err < us_err_min) us_err_min = us_err;
|
||
if (us_err > us_err_max) us_err_max = us_err;
|
||
us_err_avg = (us_err_avg*(loopcount-1)+us_err)/loopcount;
|
||
us_err_rms = sqrt(((us_err_rms)*(us_err_rms)*(loopcount-1)+us_err*us_err)/loopcount);
|
||
}
|
||
if (!(loopcount%1)){ //print every n loops
|
||
printf("%+5.2f [us] Min: %+5.2f [us] Max:%+5.2f [us] Avg:%+5.2f [us] RMS: %5.2f [us]\n",us_err, us_err_min, us_err_max, us_err_avg, us_err_rms);
|
||
}
|
||
|
||
|
||
|
||
|
||
//do stuff here:
|
||
|
||
// stop loop after n cycles
|
||
if (loopcount > 10000) loopok = 0;
|
||
|
||
|
||
|
||
//Loop Timing
|
||
t.tv_nsec += period; //add a period to the cycle
|
||
tsnorm(&t); //reformat to ns
|
||
//The famous sleep until next tick:
|
||
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, &t);
|
||
}
|
||
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;
|
||
}
|