Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/hardware/blmc_drivers/src/motor_board.cpp	Lines:	1	177	0.6 %
Date:	2020-04-15 11:50:02	Branches:	2	167	1.2 %


/**
 * @file motor_board.cpp
 * @author Felix Widmaier (felix.widmaier@tuebingen.mpg.de)
 * @author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * @author Maximilien Naveau (maximilien.naveau@gmail.com)
 * @brief This file implements the classes from
 * "blmc_drivers/devices/motor_board.hpp"
 * @version 0.1
 * @date 2018-11-26
 *
 * @copyright Copyright (c) 2018
 *
 */

#include <blmc_drivers/devices/motor_board.hpp>

namespace blmc_drivers
{

CanBusMotorBoard::CanBusMotorBoard(
        std::shared_ptr<CanBusInterface> can_bus,
        const size_t& history_length,
        const int& control_timeout_ms):
    can_bus_(can_bus),
    motors_are_paused_(false),
    control_timeout_ms_(control_timeout_ms)
{
    measurement_  = create_vector_of_pointers<ScalarTimeseries>(
                measurement_count,
                history_length);
    status_       = std::make_shared<StatusTimeseries>(history_length);
    control_      = create_vector_of_pointers<ScalarTimeseries>(
                control_count,
                history_length);
    command_      = std::make_shared<CommandTimeseries>(history_length);
    sent_control_ = create_vector_of_pointers<ScalarTimeseries>(
                control_count,
                history_length);
    sent_command_ = std::make_shared<CommandTimeseries>(history_length);

    is_loop_active_ = true;
    rt_thread_.create_realtime_thread(&CanBusMotorBoard::loop, this);
}

CanBusMotorBoard::~CanBusMotorBoard()
{
    is_loop_active_ = false;
    rt_thread_.join();
    set_command(MotorBoardCommand(MotorBoardCommand::IDs::ENABLE_SYS,
                                  MotorBoardCommand::Contents::DISABLE));
    send_newest_command();
}

void CanBusMotorBoard::send_if_input_changed()
{
    // send command if a new one has been set ----------------------------------
    if(command_->has_changed_since_tag())
    {
        send_newest_command();
    }

    // send controls if a new one has been set ---------------------------------
    bool controls_have_changed = false;

    for(auto control : control_)
    {
        if(control->has_changed_since_tag())
            controls_have_changed = true;
    }
    if(controls_have_changed)
    {
        send_newest_controls();
    }
}


void CanBusMotorBoard::wait_until_ready()
{
    rt_printf("waiting for board and motors to be ready \n");
    real_time_tools::ThreadsafeTimeseries<>::Index time_index = status_->newest_timeindex();
    bool is_ready = false;
    while(!is_ready)
    {
        MotorBoardStatus status = (*status_)[time_index];
        time_index++;

        is_ready = status.is_ready();
    }
    rt_printf("board and motors are ready \n");
}


bool CanBusMotorBoard::is_ready()
{
    if(status_->length() == 0)
    {
        return false;
    }
    else
    {
        return status_->newest_element().is_ready();
    }
}

void CanBusMotorBoard::pause_motors()
{
    set_control(0, current_target_0);
    set_control(0, current_target_1);
    send_newest_controls();

    set_command(MotorBoardCommand(MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT,
                                  MotorBoardCommand::Contents::DISABLE));
    send_newest_command();

    motors_are_paused_ = true;
}

void CanBusMotorBoard::disable_can_recv_timeout()
{
    set_command(MotorBoardCommand(MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT,
                                  MotorBoardCommand::Contents::DISABLE));
    send_newest_command();
}


void CanBusMotorBoard::send_newest_controls()
{
    if(motors_are_paused_)
    {
        set_command(MotorBoardCommand(
                        MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT,
                        control_timeout_ms_));
        send_newest_command();
        motors_are_paused_ = false;
    }

    std::array<double, 2> controls;
    for(size_t i = 0; i < control_.size(); i++)
    {
        if(control_[i]->length() == 0)
        {
            rt_printf("you tried to send control but no control has been set\n");
            exit(-1);
        }

        Index timeindex = control_[i]->newest_timeindex();
        controls[i] = (*control_[i])[timeindex];
        control_[i]->tag(timeindex);

        sent_control_[i]->append(controls[i]);
    }

    float current_mtr1 = controls[0];
    float current_mtr2 = controls[1];

    uint8_t data[8];
    uint32_t q_current1, q_current2;

    // Convert floats to Q24 values
    q_current1 = float_to_q24(current_mtr1);
    q_current2 = float_to_q24(current_mtr2);

    // Motor 1
    data[0] = (q_current1 >> 24) & 0xFF;
    data[1] = (q_current1 >> 16) & 0xFF;
    data[2] = (q_current1 >> 8) & 0xFF;
    data[3] =  q_current1 & 0xFF;

    // Motor 2
    data[4] = (q_current2 >> 24) & 0xFF;
    data[5] = (q_current2 >> 16) & 0xFF;
    data[6] = (q_current2 >> 8) & 0xFF;
    data[7] =  q_current2 & 0xFF;

    CanBusFrame can_frame;
    can_frame.id = CanframeIDs::IqRef;
    for(size_t i = 0; i < 7; i++)
    {
        can_frame.data[i] = data[i];
    }
    can_frame.dlc = 8;

    can_bus_->set_input_frame(can_frame);
    can_bus_->send_if_input_changed();
}

void CanBusMotorBoard::send_newest_command()
{
    if(command_->length() == 0)
    {
        rt_printf("you tried to send command but no command has been set\n");
        exit(-1);
    }

    Index timeindex = command_->newest_timeindex();
    MotorBoardCommand command = (*command_)[timeindex];
    command_->tag(timeindex);
    sent_command_->append(command);

    uint32_t id = command.id_;
    int32_t content = command.content_;

    uint8_t data[8];

    // content
    data[0] = (content >> 24) & 0xFF;
    data[1] = (content >> 16) & 0xFF;
    data[2] = (content >> 8) & 0xFF;
    data[3] = content & 0xFF;

    // command
    data[4] = (id >> 24) & 0xFF;
    data[5] = (id >> 16) & 0xFF;
    data[6] = (id >> 8) & 0xFF;
    data[7] = id & 0xFF;

    CanBusFrame can_frame;
    can_frame.id = CanframeIDs::COMMAND_ID;
    for(size_t i = 0; i < 8; i++)
    {
        can_frame.data[i] = data[i];
    }
    can_frame.dlc = 8;

    can_bus_->set_input_frame(can_frame);
    can_bus_->send_if_input_changed();
}

void CanBusMotorBoard::loop()
{
    pause_motors();

    // initialize board --------------------------------------------------------
    set_command(MotorBoardCommand(
                    MotorBoardCommand::IDs::ENABLE_SYS,
                    MotorBoardCommand::Contents::ENABLE));
    send_newest_command();

    set_command(MotorBoardCommand(
                    MotorBoardCommand::IDs::SEND_ALL,
                    MotorBoardCommand::Contents::ENABLE));
    send_newest_command();

    set_command(MotorBoardCommand(
                    MotorBoardCommand::IDs::ENABLE_MTR1,
                    MotorBoardCommand::Contents::ENABLE));
    send_newest_command();

    set_command(MotorBoardCommand(
                    MotorBoardCommand::IDs::ENABLE_MTR2,
                    MotorBoardCommand::Contents::ENABLE));
    send_newest_command();

    // receive data from board in a loop ---------------------------------------
    long int timeindex = can_bus_->get_output_frame()->newest_timeindex();
    while(is_loop_active_)
    {
        CanBusFrame can_frame;
        Index received_timeindex = timeindex;
        can_frame = (*can_bus_->get_output_frame())[received_timeindex];

        if(received_timeindex != timeindex)
        {
            rt_printf("did not get the timeindex we expected! "
                      "received_timeindex: %d, "
                      "desired_timeindex: %d\n",
                      int(received_timeindex), int(timeindex));
            exit(-1);
        }

        timeindex++;

        // convert to measurement ------------------------------------------
        double measurement_0 = qbytes_to_float(can_frame.data.begin());
        double measurement_1 = qbytes_to_float((can_frame.data.begin() + 4));


        switch(can_frame.id)
        {
        case CanframeIDs::Iq:
            measurement_[current_0]->append(measurement_0);
            measurement_[current_1]->append(measurement_1);
            break;
        case CanframeIDs::POS:
            // Convert the position unit from the blmc card (kilo-rotations)
            // into rad.
            measurement_[position_0]->append(measurement_0 * 2 * M_PI);
            measurement_[position_1]->append(measurement_1 * 2 * M_PI);
            break;
        case CanframeIDs::SPEED:
            // Convert the speed unit from the blmc card (kilo-rotations-per-minutes)
            // into rad/s.
            measurement_[velocity_0]->append(measurement_0 * 2 * M_PI * (1000./60.));
            measurement_[velocity_1]->append(measurement_1 * 2 * M_PI * (1000./60.));
            break;
        case CanframeIDs::ADC6:
            measurement_[analog_0]->append(measurement_0);
            measurement_[analog_1]->append(measurement_1);
            break;
        case CanframeIDs::ENC_INDEX:
        {
            // here the interpretation of the message is different,
            // we get a motor index and a measurement
            uint8_t motor_index = can_frame.data[4];
            if(motor_index == 0)
            {
                measurement_[encoder_index_0]->append(measurement_0 * 2 * M_PI);
            }
            else if(motor_index == 1)
            {
                measurement_[encoder_index_1]->append(measurement_0 * 2 * M_PI);
            }
            else
            {
                rt_printf("ERROR: Invalid motor number"
                          "for encoder index: %d\n", motor_index);
                exit(-1);
            }
            break;
        }
        case CanframeIDs::STATUSMSG:
        {
            MotorBoardStatus status;
            uint8_t data = can_frame.data[0];
            status.system_enabled = data >> 0;
            status.motor1_enabled = data >> 1;
            status.motor1_ready   = data >> 2;
            status.motor2_enabled = data >> 3;
            status.motor2_ready   = data >> 4;
            status.error_code     = data >> 5;

            status_->append(status);
            break;
        }
        }

        //        static int count = 0;
        //        if(count % 4000 == 0)
        //        {
        //            print_status();
        //        }
        //        count++;
    }
}

void CanBusMotorBoard::print_status()
{
    rt_printf("ouptus ======================================\n");
    rt_printf("measurements: -------------------------------\n");
    for(size_t i = 0; i < measurement_.size(); i++)
    {
        rt_printf("%d: ---------------------------------\n", int(i));
        if(measurement_[i]->length() > 0)
        {
            double measurement = measurement_[i]->newest_element();
            rt_printf("value %f:\n", measurement);
        }
    }

    rt_printf("status: ---------------------------------\n");
    if(status_->length() > 0)
        status_->newest_element().print();

    //        rt_printf("inputs ======================================\n");

    //        for(size_t i = 0; i < control_names.size(); i++)
    //        {
    //            rt_printf("%s: ---------------------------------\n",
    //                                 control_names[i].c_str());
    //            if(control_.at(control_names[i])->length() > 0)
    //            {
    //                double control =
    //                        control_.at(control_names[i])->newest_element();
    //                rt_printf("value %f:\n", control);
    //            }
    //        }

    //        rt_printf("command: ---------------------------------\n");
    //        if(command_[command]->length() > 0)
    //            command_[command]->newest_element().print();
}

} // namespace blmc_drivers


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/**
2			* @file motor_board.cpp
3			* @author Felix Widmaier (felix.widmaier@tuebingen.mpg.de)
4			* @author Manuel Wuthrich (manuel.wuthrich@gmail.com)
5			* @author Maximilien Naveau (maximilien.naveau@gmail.com)
6			* @brief This file implements the classes from
7			* "blmc_drivers/devices/motor_board.hpp"
8			* @version 0.1
9			* @date 2018-11-26
10			*
11			* @copyright Copyright (c) 2018
12			*
13			*/
14
15			#include <blmc_drivers/devices/motor_board.hpp>
16
17			namespace blmc_drivers
18			{
19
20			CanBusMotorBoard::CanBusMotorBoard(
21			std::shared_ptr<CanBusInterface> can_bus,
22			const size_t& history_length,
23			const int& control_timeout_ms):
24			can_bus_(can_bus),
25			motors_are_paused_(false),
26			control_timeout_ms_(control_timeout_ms)
27			{
28			measurement_ = create_vector_of_pointers<ScalarTimeseries>(
29			measurement_count,
30			history_length);
31			status_ = std::make_shared<StatusTimeseries>(history_length);
32			control_ = create_vector_of_pointers<ScalarTimeseries>(
33			control_count,
34			history_length);
35			command_ = std::make_shared<CommandTimeseries>(history_length);
36			sent_control_ = create_vector_of_pointers<ScalarTimeseries>(
37			control_count,
38			history_length);
39			sent_command_ = std::make_shared<CommandTimeseries>(history_length);
40
41			is_loop_active_ = true;
42			rt_thread_.create_realtime_thread(&CanBusMotorBoard::loop, this);
43			}
44
45			CanBusMotorBoard::~CanBusMotorBoard()
46			{
47			is_loop_active_ = false;
48			rt_thread_.join();
49			set_command(MotorBoardCommand(MotorBoardCommand::IDs::ENABLE_SYS,
50			MotorBoardCommand::Contents::DISABLE));
51			send_newest_command();
52			}
53
54			void CanBusMotorBoard::send_if_input_changed()
55			{
56			// send command if a new one has been set ----------------------------------
57			if(command_->has_changed_since_tag())
58			{
59			send_newest_command();
60			}
61
62			// send controls if a new one has been set ---------------------------------
63			bool controls_have_changed = false;
64
65			for(auto control : control_)
66			{
67			if(control->has_changed_since_tag())
68			controls_have_changed = true;
69			}
70			if(controls_have_changed)
71			{
72			send_newest_controls();
73			}
74			}
75
76
77			void CanBusMotorBoard::wait_until_ready()
78			{
79			rt_printf("waiting for board and motors to be ready \n");
80			real_time_tools::ThreadsafeTimeseries<>::Index time_index = status_->newest_timeindex();
81			bool is_ready = false;
82			while(!is_ready)
83			{
84			MotorBoardStatus status = (*status_)[time_index];
85			time_index++;
86
87			is_ready = status.is_ready();
88			}
89			rt_printf("board and motors are ready \n");
90			}
91
92
93			bool CanBusMotorBoard::is_ready()
94			{
95			if(status_->length() == 0)
96			{
97			return false;
98			}
99			else
100			{
101			return status_->newest_element().is_ready();
102			}
103			}
104
105			void CanBusMotorBoard::pause_motors()
106			{
107			set_control(0, current_target_0);
108			set_control(0, current_target_1);
109			send_newest_controls();
110
111			set_command(MotorBoardCommand(MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT,
112			MotorBoardCommand::Contents::DISABLE));
113			send_newest_command();
114
115			motors_are_paused_ = true;
116			}
117
118			void CanBusMotorBoard::disable_can_recv_timeout()
119			{
120			set_command(MotorBoardCommand(MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT,
121			MotorBoardCommand::Contents::DISABLE));
122			send_newest_command();
123			}
124
125
126			void CanBusMotorBoard::send_newest_controls()
127			{
128			if(motors_are_paused_)
129			{
130			set_command(MotorBoardCommand(
131			MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT,
132			control_timeout_ms_));
133			send_newest_command();
134			motors_are_paused_ = false;
135			}
136
137			std::array<double, 2> controls;
138			for(size_t i = 0; i < control_.size(); i++)
139			{
140			if(control_[i]->length() == 0)
141			{
142			rt_printf("you tried to send control but no control has been set\n");
143			exit(-1);
144			}
145
146			Index timeindex = control_[i]->newest_timeindex();
147			controls[i] = (*control_[i])[timeindex];
148			control_[i]->tag(timeindex);
149
150			sent_control_[i]->append(controls[i]);
151			}
152
153			float current_mtr1 = controls[0];
154			float current_mtr2 = controls[1];
155
156			uint8_t data[8];
157			uint32_t q_current1, q_current2;
158
159			// Convert floats to Q24 values
160			q_current1 = float_to_q24(current_mtr1);
161			q_current2 = float_to_q24(current_mtr2);
162
163			// Motor 1
164			data[0] = (q_current1 >> 24) & 0xFF;
165			data[1] = (q_current1 >> 16) & 0xFF;
166			data[2] = (q_current1 >> 8) & 0xFF;
167			data[3] = q_current1 & 0xFF;
168
169			// Motor 2
170			data[4] = (q_current2 >> 24) & 0xFF;
171			data[5] = (q_current2 >> 16) & 0xFF;
172			data[6] = (q_current2 >> 8) & 0xFF;
173			data[7] = q_current2 & 0xFF;
174
175			CanBusFrame can_frame;
176			can_frame.id = CanframeIDs::IqRef;
177			for(size_t i = 0; i < 7; i++)
178			{
179			can_frame.data[i] = data[i];
180			}
181			can_frame.dlc = 8;
182
183			can_bus_->set_input_frame(can_frame);
184			can_bus_->send_if_input_changed();
185			}
186
187			void CanBusMotorBoard::send_newest_command()
188			{
189			if(command_->length() == 0)
190			{
191			rt_printf("you tried to send command but no command has been set\n");
192			exit(-1);
193			}
194
195			Index timeindex = command_->newest_timeindex();
196			MotorBoardCommand command = (*command_)[timeindex];
197			command_->tag(timeindex);
198			sent_command_->append(command);
199
200			uint32_t id = command.id_;
201			int32_t content = command.content_;
202
203			uint8_t data[8];
204
205			// content
206			data[0] = (content >> 24) & 0xFF;
207			data[1] = (content >> 16) & 0xFF;
208			data[2] = (content >> 8) & 0xFF;
209			data[3] = content & 0xFF;
210
211			// command
212			data[4] = (id >> 24) & 0xFF;
213			data[5] = (id >> 16) & 0xFF;
214			data[6] = (id >> 8) & 0xFF;
215			data[7] = id & 0xFF;
216
217			CanBusFrame can_frame;
218			can_frame.id = CanframeIDs::COMMAND_ID;
219			for(size_t i = 0; i < 8; i++)
220			{
221			can_frame.data[i] = data[i];
222			}
223			can_frame.dlc = 8;
224
225			can_bus_->set_input_frame(can_frame);
226			can_bus_->send_if_input_changed();
227			}
228
229			void CanBusMotorBoard::loop()
230			{
231			pause_motors();
232
233			// initialize board --------------------------------------------------------
234			set_command(MotorBoardCommand(
235			MotorBoardCommand::IDs::ENABLE_SYS,
236			MotorBoardCommand::Contents::ENABLE));
237			send_newest_command();
238
239			set_command(MotorBoardCommand(
240			MotorBoardCommand::IDs::SEND_ALL,
241			MotorBoardCommand::Contents::ENABLE));
242			send_newest_command();
243
244			set_command(MotorBoardCommand(
245			MotorBoardCommand::IDs::ENABLE_MTR1,
246			MotorBoardCommand::Contents::ENABLE));
247			send_newest_command();
248
249			set_command(MotorBoardCommand(
250			MotorBoardCommand::IDs::ENABLE_MTR2,
251			MotorBoardCommand::Contents::ENABLE));
252			send_newest_command();
253
254			// receive data from board in a loop ---------------------------------------
255			long int timeindex = can_bus_->get_output_frame()->newest_timeindex();
256			while(is_loop_active_)
257			{
258			CanBusFrame can_frame;
259			Index received_timeindex = timeindex;
260			can_frame = (*can_bus_->get_output_frame())[received_timeindex];
261
262			if(received_timeindex != timeindex)
263			{
264			rt_printf("did not get the timeindex we expected! "
265			"received_timeindex: %d, "
266			"desired_timeindex: %d\n",
267			int(received_timeindex), int(timeindex));
268			exit(-1);
269			}
270
271			timeindex++;
272
273			// convert to measurement ------------------------------------------
274			double measurement_0 = qbytes_to_float(can_frame.data.begin());
275			double measurement_1 = qbytes_to_float((can_frame.data.begin() + 4));
276
277
278			switch(can_frame.id)
279			{
280			case CanframeIDs::Iq:
281			measurement_[current_0]->append(measurement_0);
282			measurement_[current_1]->append(measurement_1);
283			break;
284			case CanframeIDs::POS:
285			// Convert the position unit from the blmc card (kilo-rotations)
286			// into rad.
287			measurement_[position_0]->append(measurement_0 * 2 * M_PI);
288			measurement_[position_1]->append(measurement_1 * 2 * M_PI);
289			break;
290			case CanframeIDs::SPEED:
291			// Convert the speed unit from the blmc card (kilo-rotations-per-minutes)
292			// into rad/s.
293			measurement_[velocity_0]->append(measurement_0 * 2 * M_PI * (1000./60.));
294			measurement_[velocity_1]->append(measurement_1 * 2 * M_PI * (1000./60.));
295			break;
296			case CanframeIDs::ADC6:
297			measurement_[analog_0]->append(measurement_0);
298			measurement_[analog_1]->append(measurement_1);
299			break;
300			case CanframeIDs::ENC_INDEX:
301			{
302			// here the interpretation of the message is different,
303			// we get a motor index and a measurement
304			uint8_t motor_index = can_frame.data[4];
305			if(motor_index == 0)
306			{
307			measurement_[encoder_index_0]->append(measurement_0 * 2 * M_PI);
308			}
309			else if(motor_index == 1)
310			{
311			measurement_[encoder_index_1]->append(measurement_0 * 2 * M_PI);
312			}
313			else
314			{
315			rt_printf("ERROR: Invalid motor number"
316			"for encoder index: %d\n", motor_index);
317			exit(-1);
318			}
319			break;
320			}
321			case CanframeIDs::STATUSMSG:
322			{
323			MotorBoardStatus status;
324			uint8_t data = can_frame.data[0];
325			status.system_enabled = data >> 0;
326			status.motor1_enabled = data >> 1;
327			status.motor1_ready = data >> 2;
328			status.motor2_enabled = data >> 3;
329			status.motor2_ready = data >> 4;
330			status.error_code = data >> 5;
331
332			status_->append(status);
333			break;
334			}
335			}
336
337			// static int count = 0;
338			// if(count % 4000 == 0)
339			// {
340			// print_status();
341			// }
342			// count++;
343			}
344			}
345
346			void CanBusMotorBoard::print_status()
347			{
348			rt_printf("ouptus ======================================\n");
349			rt_printf("measurements: -------------------------------\n");
350			for(size_t i = 0; i < measurement_.size(); i++)
351			{
352			rt_printf("%d: ---------------------------------\n", int(i));
353			if(measurement_[i]->length() > 0)
354			{
355			double measurement = measurement_[i]->newest_element();
356			rt_printf("value %f:\n", measurement);
357			}
358			}
359
360			rt_printf("status: ---------------------------------\n");
361			if(status_->length() > 0)
362			status_->newest_element().print();
363
364			// rt_printf("inputs ======================================\n");
365
366			// for(size_t i = 0; i < control_names.size(); i++)
367			// {
368			// rt_printf("%s: ---------------------------------\n",
369			// control_names[i].c_str());
370			// if(control_.at(control_names[i])->length() > 0)
371			// {
372			// double control =
373			// control_.at(control_names[i])->newest_element();
374			// rt_printf("value %f:\n", control);
375			// }
376			// }
377
378			// rt_printf("command: ---------------------------------\n");
379			// if(command_[command]->length() > 0)
380			// command_[command]->newest_element().print();
381			}
382
383	✓✗✓✗	3	} // namespace blmc_drivers