distance-robot-control/velocity_response.cpp at main · francesco1203/distance-robot-control · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

#include "meca500_ethercat_cpp/Robot.hpp"
#include "csvlogger/CsvLogger.hpp"
#include <pigpio.h>
#include <unistd.h>

int main(int argc, char *argv[])
{
    if (gpioInitialise() < 0)
        exit(1);

    /*robot, setup*/
    Robot robot(40, 240, 5000, "eth0", 0.0, 10);
    robot.reset_error();
    // robot.main();

    robot.set_conf(1, 1, -1);
    robot.move_pose(130, -170, 120, 90, 90, 0);
    robot.print_pose();

    /*files to write, setup*/
    CsvLogger input_logger("control/velocity_control");
    input_logger.write("time, value\n");

    CsvLogger output_velocity_logger("control/velocity_response");
    output_velocity_logger.write("time, value\n");

    CsvLogger output_position_logger("control/position_response");
    output_position_logger.write("time, value\n");

    /*squarewave generation, setup*/
    float input_velocity_mms = 10; // 10mm/s    (amplitude)
    double period_s = 6;           // 6sec      (period)
    float velocity[6] = {0, 0, 0, 0, 0, 0};
    /*squarewawe T = 6s, A=10mm/s*/

    /*time variables setup*/
    uint32_t startTime, currentTime, t0;
    t0 = gpioTick(); // time to start analysing response

    /*process*/
    while (true)
    {
        velocity[0] = input_velocity_mms;
        // input

        startTime = gpioTick();
        currentTime = gpioTick();
        while (currentTime - startTime >= period_s * 1e6 / 2) // Run the loop for 3 seconds
        {

            robot.move_lin_vel_wrf(velocity); // give 10mm/s or -10mm/s

            // Check if 3 secone passed. Exit cycle if true
            currentTime = gpioTick();

            // get datas
            input_logger << (currentTime - t0) / 1e6;
            input_logger << input_velocity_mms;
            input_logger.end_row();

            output_velocity_logger << (currentTime - t0) / 1e6;
            output_velocity_logger << robot.get_velocity();
            output_velocity_logger.end_row();

            output_position_logger << (currentTime - t0) / 1e6;
            output_position_logger << robot.get_position();
            output_position_logger.end_row();

            // short delay:
            usleep(1000);
        }

        input_velocity_mms = -input_velocity_mms;
    }
}