Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/hardware/blmc_drivers/src/spi_bus.cpp	Lines:	1	210	0.5 %
Date:	2020-04-15 11:50:02	Branches:	2	238	0.8 %


/**
 * @file spi_bus.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) 2020, New York University and Max Planck Gesellshaft.
 *
 */

#include "real_time_tools/timer.hpp"
#include "blmc_drivers/devices/motor.hpp"
#include "blmc_drivers/devices/spi_bus.hpp"


namespace rt = real_time_tools;

namespace blmc_drivers
{

SpiBus::SpiBus(
    std::shared_ptr<MasterBoardInterface> main_board_interface,
    const size_t& nb_udrivers, const size_t& history_length): DeviceInterface()
{
  // Save the ser input
  main_board_interface_ = main_board_interface;
  nb_udrivers_ = nb_udrivers;
  history_length_ = history_length;

  // get the udriver from its index:
  assert(N_SLAVES >= nb_udrivers_ && "The number of controlled udriver is not valid.");

  // initialize the buffers
  measurement_ = create_vector_of_pointers<MotorInterface::ScalarTimeseries>(
      nb_udrivers_ * MotorBoardInterface::MeasurementIndex::measurement_count,
      history_length_);

  status_ = create_vector_of_pointers<MotorBoardInterface::StatusTimeseries>(
      nb_udrivers_,
      history_length_);

  control_ = create_vector_of_pointers<MotorInterface::ScalarTimeseries>(
      nb_udrivers_ * MotorBoardInterface::ControlIndex::control_count,
      history_length);

  command_ = create_vector_of_pointers<MotorBoardInterface::CommandTimeseries>(
      nb_udrivers_, history_length);

  sent_control_ = create_vector_of_pointers<MotorInterface::ScalarTimeseries>(
      nb_udrivers_ * MotorBoardInterface::ControlIndex::control_count,
      history_length);

  sent_command_ = create_vector_of_pointers<MotorBoardInterface::CommandTimeseries>(
      nb_udrivers_, history_length);

  // start the communication loop
  motors_are_paused_ = true;
  is_loop_active_ = true;
  rt_thread_.create_realtime_thread(&SpiBus::loop, this);
}

SpiBus::~SpiBus()
{
    is_loop_active_ = false;
    rt_thread_.join();
    // de-activate the robot
    for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
    {
        // Some aliases
        MotorDriver& udriver = main_board_interface_->motor_drivers[i];
        udriver.motor1->SetCurrentReference(0);
        udriver.motor2->SetCurrentReference(0);
        udriver.motor1->Disable();
        udriver.motor2->Disable();
        udriver.DisablePositionRolloverError();
        udriver.Disable();
    }
}

/**
 * Output and status
 */

std::shared_ptr<const MotorBoardInterface::ScalarTimeseries>
SpiBus::get_measurement(
    const size_t udriver_id,
    const MotorBoardInterface::MeasurementIndex& index) const
{
    return measurement_[udriver_id *
        MotorBoardInterface::MeasurementIndex::measurement_count + index];
}

std::shared_ptr<const MotorBoardInterface::StatusTimeseries>
SpiBus::get_status(const size_t udriver_id) const
{
    return status_[udriver_id];
}

/**
 * input logs
 */

std::shared_ptr<const MotorBoardInterface::ScalarTimeseries>
SpiBus::get_control(const size_t udriver_id,
    const MotorBoardInterface::ControlIndex& index) const
{
    return control_[udriver_id *
        MotorBoardInterface::ControlIndex::control_count + index];
}

std::shared_ptr<const MotorBoardInterface::CommandTimeseries>
SpiBus::get_command(const size_t udriver_id) const
{
    return command_[udriver_id];
}

std::shared_ptr<const MotorBoardInterface::ScalarTimeseries>
SpiBus::get_sent_control(const size_t udriver_id,
    const MotorBoardInterface::ControlIndex& index) const
{
    return control_[udriver_id *
        MotorBoardInterface::ControlIndex::control_count + index];
}

std::shared_ptr<const MotorBoardInterface::CommandTimeseries>
SpiBus::get_sent_command(const size_t udriver_id) const
{
    return sent_command_[udriver_id];
}

/**
 * Setters
 */

void SpiBus::set_control(const size_t udriver_id, const double& control,
                         const MotorBoardInterface::ControlIndex& index)
{
    control_[udriver_id * MotorBoardInterface::ControlIndex::control_count +
             index]->append(control);
}

void SpiBus::set_command(const size_t udriver_id,
                         const MotorBoardCommand& command)
{
    command_[udriver_id]->append(command);
}

void SpiBus::send_if_input_changed()
{
    // send command if a new one has been set ----------------------------------
    bool commands_have_changed = false;
    for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
    {
        if(command_[i]->has_changed_since_tag())
        {
            commands_have_changed = true;
            break;
        }
    }
    if(commands_have_changed)
    {
        send_newest_command();
    }
    // send controls if a new one has been set ---------------------------------
    bool controls_have_changed = false;
    for(size_t i = 0 ; i < control_.size() ; ++i)
    {
        if(control_[i]->has_changed_since_tag())
        {
            controls_have_changed = true;
            break;
        }
    }
    if(controls_have_changed)
    {
        send_newest_controls();
    }
}

void SpiBus::send_newest_command()
{
    for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
    {
        MotorInterface::ScalarTimeseries::Index timeindex =
            command_[i]->newest_timeindex();
        MotorBoardCommand command = (*command_[i])[timeindex];
        command_[i]->tag(timeindex);
        sent_command_[i]->append(command);

        // Some aliases
        MotorDriver& udriver = main_board_interface_->motor_drivers[i];
        const uint32_t& command_id = command_[i]->newest_element().id_;
        const int32_t& command_content = command_[i]->newest_element().content_;
        switch (command_id)
        {
            case MotorBoardCommand::ENABLE_SYS:
                command_content == MotorBoardCommand::ENABLE ?
                    udriver.Enable() : udriver.Disable();
                break;
            case MotorBoardCommand::ENABLE_MTR1:
                command_content == MotorBoardCommand::ENABLE ?
                    udriver.motor1->Enable() : udriver.motor1->Disable();
                break;
            case MotorBoardCommand::ENABLE_MTR2:
                command_content == MotorBoardCommand::ENABLE ?
                    udriver.motor2->Enable() : udriver.motor2->Disable();
                break;
            case MotorBoardCommand::ENABLE_VSPRING1:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::ENABLE_VSPRING1 no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::ENABLE_VSPRING2:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::ENABLE_VSPRING2 no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SEND_CURRENT:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_CURRENT no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SEND_POSITION:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_POSITION no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SEND_VELOCITY:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_VELOCITY no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SEND_ADC6:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_ADC6 no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SEND_ENC_INDEX:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_ENC_INDEX no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SEND_ALL:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_ALL no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::SET_CAN_RECV_TIMEOUT:
                rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SET_CAN_RECV_TIMEOUT no implemented. Nothing to be done.");
                break;
            case MotorBoardCommand::ENABLE_POS_ROLLOVER_ERROR:
                command_content == MotorBoardCommand::ENABLE ?
                    udriver.EnablePositionRolloverError() :
                    udriver.DisablePositionRolloverError();
                break;
            default:
                throw std::runtime_error("SpiBus::send_if_input_changed:"
                                        " Error cannot send unkown command");
                break;
        } // end switch
    } // end for
}

void SpiBus::send_newest_controls()
{
    // send controls -----------------------------------------------------------
    for(size_t i = 0; i < control_.size(); ++i)
    {
        if(control_[i]->length() == 0)
        {
            rt_printf("You tried to send control on the motor[%ld] "
                      "but no control has been set\n", i);
            exit(-1);
        }
        MotorInterface::ScalarTimeseries::Index timeindex =
            control_[i]->newest_timeindex();

        double control = (*control_[i])[timeindex];

        main_board_interface_->motors[i].SetCurrentReference(control);
        control_[i]->tag(timeindex);
        sent_control_[i]->append(control);
    }
    motors_are_paused_ = false;
    main_board_interface_->SendCommand();
}

bool SpiBus::is_ready()
{
    // check if the motors and if the udriver are enabled and ready.
    bool is_hardware_ready = true;
    for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
    {
        MotorDriver& udriver = main_board_interface_->motor_drivers[i];
        is_hardware_ready = is_hardware_ready &&
                            udriver.motor1->IsEnabled() &&
                            udriver.motor2->IsEnabled() &&
                            udriver.motor1->IsReady() &&
                            udriver.motor2->IsReady();
    }
    return is_hardware_ready;
}

void SpiBus::loop()
{
    motors_are_paused_ = true;

    for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
    {
        MotorDriver& udriver = main_board_interface_->motor_drivers[i];
        // Initialisation of the udriver, send the init commands
        udriver.motor1->SetCurrentReference(0);
        udriver.motor2->SetCurrentReference(0);
        udriver.motor1->Enable();
        udriver.motor2->Enable();
        udriver.EnablePositionRolloverError();
        udriver.SetTimeout(5);
        udriver.Enable();
    }

    // some aliases
    size_t measurement_count = MotorBoardInterface::MeasurementIndex::measurement_count;
    size_t current_0 = MotorBoardInterface::MeasurementIndex::current_0;
    size_t current_1 = MotorBoardInterface::MeasurementIndex::current_1;
    size_t position_0 = MotorBoardInterface::MeasurementIndex::position_0;
    size_t position_1 = MotorBoardInterface::MeasurementIndex::position_1;
    size_t velocity_0 = MotorBoardInterface::MeasurementIndex::velocity_0;
    size_t velocity_1 = MotorBoardInterface::MeasurementIndex::velocity_1;
    size_t analog_0 = MotorBoardInterface::MeasurementIndex::analog_0;
    size_t analog_1 = MotorBoardInterface::MeasurementIndex::analog_1;
    size_t encoder_index_0 = MotorBoardInterface::MeasurementIndex::encoder_index_0;
    size_t encoder_index_1 = MotorBoardInterface::MeasurementIndex::encoder_index_1;
    // receive data from board in a loop ---------------------------------------
    rt::Spinner spinner;
    spinner.set_period(0.001); // period of 1ms
    while(is_loop_active_)
    {
        // This will read the last incomming packet and update all sensor fields.
        main_board_interface_->ParseSensorData();

        for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
        {
            MotorDriver& udriver = main_board_interface_->motor_drivers[i];
            measurement_[i* measurement_count + current_0]->append(
                  udriver.motor1->GetCurrent());
            measurement_[i* measurement_count + current_1]->append(
                  udriver.motor2->GetCurrent());
            measurement_[i* measurement_count + position_0]->append(
                  udriver.motor1->GetPosition() /* * 2 * M_PI */);
            measurement_[i* measurement_count + position_1]->append(
                  udriver.motor2->GetPosition() /* * 2 * M_PI */);
            measurement_[i* measurement_count + velocity_0]->append(
                  udriver.motor1->GetVelocity() /* * 2 * M_PI * (1000./60.) */);
            measurement_[i* measurement_count + velocity_1]->append(
                  udriver.motor2->GetVelocity() /* * 2 * M_PI * (1000./60.) */);
            measurement_[i* measurement_count + analog_0]->append(
                  udriver.adc[0]);// not implemented yet in the API
            measurement_[i* measurement_count + analog_1]->append(
                  udriver.adc[1]);// not implemented yet in the API
            // here the interpretation of the message is different,
            // we get a motor index and a measurement
            if(motor_index_toggle_bits_[2 * i] !=
               udriver.motor1->GetIndexToggleBit())
            {
                measurement_[i* measurement_count + encoder_index_0]->append(
                      udriver.motor1->GetPosition() /* * 2 * M_PI */);
            }
            if(motor_index_toggle_bits_[2 * i + 1] !=
               udriver.motor2->GetIndexToggleBit())
            {
                measurement_[i* measurement_count + encoder_index_1]->append(
                      udriver.motor2->GetPosition() /* * 2 * M_PI */);
            }
            // store the last toggle bit.
            motor_index_toggle_bits_[2 * i] =
                udriver.motor1->GetIndexToggleBit();
            motor_index_toggle_bits_[2 * i + 1] =
                udriver.motor2->GetIndexToggleBit();
            MotorBoardStatus status;
            status.system_enabled = udriver.is_enabled;
            status.motor1_enabled = udriver.motor1->is_enabled;
            status.motor1_ready   = udriver.motor1->is_ready;
            status.motor2_enabled = udriver.motor2->is_enabled;
            status.motor2_ready   = udriver.motor2->is_ready;
            status.error_code     = udriver.error_code;
            status_[i]->append(status);
        }

        if(!is_ready() || motors_are_paused_)
        {
            for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
            {
                set_control(i, 0.0, MotorBoardInterface::ControlIndex::current_target_0);
                set_control(i, 0.0, MotorBoardInterface::ControlIndex::current_target_1);
                MotorDriver& udriver = main_board_interface_->motor_drivers[i];
                udriver.motor1->SetCurrentReference(0.0);
                udriver.motor2->SetCurrentReference(0.0);
            }
            main_board_interface_->SendCommand();
        }
        spinner.spin();
    }
}

void SpiBus::wait_until_ready()
{
    while(!is_ready())
    {
        rt::Timer::sleep_sec(0.001);
    }
}

} // namespace blmc_drivers


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/**
2			* @file spi_bus.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) 2020, New York University and Max Planck Gesellshaft.
12			*
13			*/
14
15			#include "real_time_tools/timer.hpp"
16			#include "blmc_drivers/devices/motor.hpp"
17			#include "blmc_drivers/devices/spi_bus.hpp"
18
19
20			namespace rt = real_time_tools;
21
22			namespace blmc_drivers
23			{
24
25			SpiBus::SpiBus(
26			std::shared_ptr<MasterBoardInterface> main_board_interface,
27			const size_t& nb_udrivers, const size_t& history_length): DeviceInterface()
28			{
29			// Save the ser input
30			main_board_interface_ = main_board_interface;
31			nb_udrivers_ = nb_udrivers;
32			history_length_ = history_length;
33
34			// get the udriver from its index:
35			assert(N_SLAVES >= nb_udrivers_ && "The number of controlled udriver is not valid.");
36
37			// initialize the buffers
38			measurement_ = create_vector_of_pointers<MotorInterface::ScalarTimeseries>(
39			nb_udrivers_ * MotorBoardInterface::MeasurementIndex::measurement_count,
40			history_length_);
41
42			status_ = create_vector_of_pointers<MotorBoardInterface::StatusTimeseries>(
43			nb_udrivers_,
44			history_length_);
45
46			control_ = create_vector_of_pointers<MotorInterface::ScalarTimeseries>(
47			nb_udrivers_ * MotorBoardInterface::ControlIndex::control_count,
48			history_length);
49
50			command_ = create_vector_of_pointers<MotorBoardInterface::CommandTimeseries>(
51			nb_udrivers_, history_length);
52
53			sent_control_ = create_vector_of_pointers<MotorInterface::ScalarTimeseries>(
54			nb_udrivers_ * MotorBoardInterface::ControlIndex::control_count,
55			history_length);
56
57			sent_command_ = create_vector_of_pointers<MotorBoardInterface::CommandTimeseries>(
58			nb_udrivers_, history_length);
59
60			// start the communication loop
61			motors_are_paused_ = true;
62			is_loop_active_ = true;
63			rt_thread_.create_realtime_thread(&SpiBus::loop, this);
64			}
65
66			SpiBus::~SpiBus()
67			{
68			is_loop_active_ = false;
69			rt_thread_.join();
70			// de-activate the robot
71			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
72			{
73			// Some aliases
74			MotorDriver& udriver = main_board_interface_->motor_drivers[i];
75			udriver.motor1->SetCurrentReference(0);
76			udriver.motor2->SetCurrentReference(0);
77			udriver.motor1->Disable();
78			udriver.motor2->Disable();
79			udriver.DisablePositionRolloverError();
80			udriver.Disable();
81			}
82			}
83
84			/**
85			* Output and status
86			*/
87
88			std::shared_ptr<const MotorBoardInterface::ScalarTimeseries>
89			SpiBus::get_measurement(
90			const size_t udriver_id,
91			const MotorBoardInterface::MeasurementIndex& index) const
92			{
93			return measurement_[udriver_id *
94			MotorBoardInterface::MeasurementIndex::measurement_count + index];
95			}
96
97			std::shared_ptr<const MotorBoardInterface::StatusTimeseries>
98			SpiBus::get_status(const size_t udriver_id) const
99			{
100			return status_[udriver_id];
101			}
102
103			/**
104			* input logs
105			*/
106
107			std::shared_ptr<const MotorBoardInterface::ScalarTimeseries>
108			SpiBus::get_control(const size_t udriver_id,
109			const MotorBoardInterface::ControlIndex& index) const
110			{
111			return control_[udriver_id *
112			MotorBoardInterface::ControlIndex::control_count + index];
113			}
114
115			std::shared_ptr<const MotorBoardInterface::CommandTimeseries>
116			SpiBus::get_command(const size_t udriver_id) const
117			{
118			return command_[udriver_id];
119			}
120
121			std::shared_ptr<const MotorBoardInterface::ScalarTimeseries>
122			SpiBus::get_sent_control(const size_t udriver_id,
123			const MotorBoardInterface::ControlIndex& index) const
124			{
125			return control_[udriver_id *
126			MotorBoardInterface::ControlIndex::control_count + index];
127			}
128
129			std::shared_ptr<const MotorBoardInterface::CommandTimeseries>
130			SpiBus::get_sent_command(const size_t udriver_id) const
131			{
132			return sent_command_[udriver_id];
133			}
134
135			/**
136			* Setters
137			*/
138
139			void SpiBus::set_control(const size_t udriver_id, const double& control,
140			const MotorBoardInterface::ControlIndex& index)
141			{
142			control_[udriver_id * MotorBoardInterface::ControlIndex::control_count +
143			index]->append(control);
144			}
145
146			void SpiBus::set_command(const size_t udriver_id,
147			const MotorBoardCommand& command)
148			{
149			command_[udriver_id]->append(command);
150			}
151
152			void SpiBus::send_if_input_changed()
153			{
154			// send command if a new one has been set ----------------------------------
155			bool commands_have_changed = false;
156			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
157			{
158			if(command_[i]->has_changed_since_tag())
159			{
160			commands_have_changed = true;
161			break;
162			}
163			}
164			if(commands_have_changed)
165			{
166			send_newest_command();
167			}
168			// send controls if a new one has been set ---------------------------------
169			bool controls_have_changed = false;
170			for(size_t i = 0 ; i < control_.size() ; ++i)
171			{
172			if(control_[i]->has_changed_since_tag())
173			{
174			controls_have_changed = true;
175			break;
176			}
177			}
178			if(controls_have_changed)
179			{
180			send_newest_controls();
181			}
182			}
183
184			void SpiBus::send_newest_command()
185			{
186			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
187			{
188			MotorInterface::ScalarTimeseries::Index timeindex =
189			command_[i]->newest_timeindex();
190			MotorBoardCommand command = (*command_[i])[timeindex];
191			command_[i]->tag(timeindex);
192			sent_command_[i]->append(command);
193
194			// Some aliases
195			MotorDriver& udriver = main_board_interface_->motor_drivers[i];
196			const uint32_t& command_id = command_[i]->newest_element().id_;
197			const int32_t& command_content = command_[i]->newest_element().content_;
198			switch (command_id)
199			{
200			case MotorBoardCommand::ENABLE_SYS:
201			command_content == MotorBoardCommand::ENABLE ?
202			udriver.Enable() : udriver.Disable();
203			break;
204			case MotorBoardCommand::ENABLE_MTR1:
205			command_content == MotorBoardCommand::ENABLE ?
206			udriver.motor1->Enable() : udriver.motor1->Disable();
207			break;
208			case MotorBoardCommand::ENABLE_MTR2:
209			command_content == MotorBoardCommand::ENABLE ?
210			udriver.motor2->Enable() : udriver.motor2->Disable();
211			break;
212			case MotorBoardCommand::ENABLE_VSPRING1:
213			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::ENABLE_VSPRING1 no implemented. Nothing to be done.");
214			break;
215			case MotorBoardCommand::ENABLE_VSPRING2:
216			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::ENABLE_VSPRING2 no implemented. Nothing to be done.");
217			break;
218			case MotorBoardCommand::SEND_CURRENT:
219			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_CURRENT no implemented. Nothing to be done.");
220			break;
221			case MotorBoardCommand::SEND_POSITION:
222			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_POSITION no implemented. Nothing to be done.");
223			break;
224			case MotorBoardCommand::SEND_VELOCITY:
225			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_VELOCITY no implemented. Nothing to be done.");
226			break;
227			case MotorBoardCommand::SEND_ADC6:
228			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_ADC6 no implemented. Nothing to be done.");
229			break;
230			case MotorBoardCommand::SEND_ENC_INDEX:
231			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_ENC_INDEX no implemented. Nothing to be done.");
232			break;
233			case MotorBoardCommand::SEND_ALL:
234			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SEND_ALL no implemented. Nothing to be done.");
235			break;
236			case MotorBoardCommand::SET_CAN_RECV_TIMEOUT:
237			rt_printf("SpiBus::send_newest_command(): Warning MotorBoardCommand::SET_CAN_RECV_TIMEOUT no implemented. Nothing to be done.");
238			break;
239			case MotorBoardCommand::ENABLE_POS_ROLLOVER_ERROR:
240			command_content == MotorBoardCommand::ENABLE ?
241			udriver.EnablePositionRolloverError() :
242			udriver.DisablePositionRolloverError();
243			break;
244			default:
245			throw std::runtime_error("SpiBus::send_if_input_changed:"
246			" Error cannot send unkown command");
247			break;
248			} // end switch
249			} // end for
250			}
251
252			void SpiBus::send_newest_controls()
253			{
254			// send controls -----------------------------------------------------------
255			for(size_t i = 0; i < control_.size(); ++i)
256			{
257			if(control_[i]->length() == 0)
258			{
259			rt_printf("You tried to send control on the motor[%ld] "
260			"but no control has been set\n", i);
261			exit(-1);
262			}
263			MotorInterface::ScalarTimeseries::Index timeindex =
264			control_[i]->newest_timeindex();
265
266			double control = (*control_[i])[timeindex];
267
268			main_board_interface_->motors[i].SetCurrentReference(control);
269			control_[i]->tag(timeindex);
270			sent_control_[i]->append(control);
271			}
272			motors_are_paused_ = false;
273			main_board_interface_->SendCommand();
274			}
275
276			bool SpiBus::is_ready()
277			{
278			// check if the motors and if the udriver are enabled and ready.
279			bool is_hardware_ready = true;
280			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
281			{
282			MotorDriver& udriver = main_board_interface_->motor_drivers[i];
283			is_hardware_ready = is_hardware_ready &&
284			udriver.motor1->IsEnabled() &&
285			udriver.motor2->IsEnabled() &&
286			udriver.motor1->IsReady() &&
287			udriver.motor2->IsReady();
288			}
289			return is_hardware_ready;
290			}
291
292			void SpiBus::loop()
293			{
294			motors_are_paused_ = true;
295
296			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
297			{
298			MotorDriver& udriver = main_board_interface_->motor_drivers[i];
299			// Initialisation of the udriver, send the init commands
300			udriver.motor1->SetCurrentReference(0);
301			udriver.motor2->SetCurrentReference(0);
302			udriver.motor1->Enable();
303			udriver.motor2->Enable();
304			udriver.EnablePositionRolloverError();
305			udriver.SetTimeout(5);
306			udriver.Enable();
307			}
308
309			// some aliases
310			size_t measurement_count = MotorBoardInterface::MeasurementIndex::measurement_count;
311			size_t current_0 = MotorBoardInterface::MeasurementIndex::current_0;
312			size_t current_1 = MotorBoardInterface::MeasurementIndex::current_1;
313			size_t position_0 = MotorBoardInterface::MeasurementIndex::position_0;
314			size_t position_1 = MotorBoardInterface::MeasurementIndex::position_1;
315			size_t velocity_0 = MotorBoardInterface::MeasurementIndex::velocity_0;
316			size_t velocity_1 = MotorBoardInterface::MeasurementIndex::velocity_1;
317			size_t analog_0 = MotorBoardInterface::MeasurementIndex::analog_0;
318			size_t analog_1 = MotorBoardInterface::MeasurementIndex::analog_1;
319			size_t encoder_index_0 = MotorBoardInterface::MeasurementIndex::encoder_index_0;
320			size_t encoder_index_1 = MotorBoardInterface::MeasurementIndex::encoder_index_1;
321			// receive data from board in a loop ---------------------------------------
322			rt::Spinner spinner;
323			spinner.set_period(0.001); // period of 1ms
324			while(is_loop_active_)
325			{
326			// This will read the last incomming packet and update all sensor fields.
327			main_board_interface_->ParseSensorData();
328
329			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
330			{
331			MotorDriver& udriver = main_board_interface_->motor_drivers[i];
332			measurement_[i* measurement_count + current_0]->append(
333			udriver.motor1->GetCurrent());
334			measurement_[i* measurement_count + current_1]->append(
335			udriver.motor2->GetCurrent());
336			measurement_[i* measurement_count + position_0]->append(
337			udriver.motor1->GetPosition() /* * 2 * M_PI */);
338			measurement_[i* measurement_count + position_1]->append(
339			udriver.motor2->GetPosition() /* * 2 * M_PI */);
340			measurement_[i* measurement_count + velocity_0]->append(
341			udriver.motor1->GetVelocity() /* * 2 * M_PI * (1000./60.) */);
342			measurement_[i* measurement_count + velocity_1]->append(
343			udriver.motor2->GetVelocity() /* * 2 * M_PI * (1000./60.) */);
344			measurement_[i* measurement_count + analog_0]->append(
345			udriver.adc[0]);// not implemented yet in the API
346			measurement_[i* measurement_count + analog_1]->append(
347			udriver.adc[1]);// not implemented yet in the API
348			// here the interpretation of the message is different,
349			// we get a motor index and a measurement
350			if(motor_index_toggle_bits_[2 * i] !=
351			udriver.motor1->GetIndexToggleBit())
352			{
353			measurement_[i* measurement_count + encoder_index_0]->append(
354			udriver.motor1->GetPosition() /* * 2 * M_PI */);
355			}
356			if(motor_index_toggle_bits_[2 * i + 1] !=
357			udriver.motor2->GetIndexToggleBit())
358			{
359			measurement_[i* measurement_count + encoder_index_1]->append(
360			udriver.motor2->GetPosition() /* * 2 * M_PI */);
361			}
362			// store the last toggle bit.
363			motor_index_toggle_bits_[2 * i] =
364			udriver.motor1->GetIndexToggleBit();
365			motor_index_toggle_bits_[2 * i + 1] =
366			udriver.motor2->GetIndexToggleBit();
367			MotorBoardStatus status;
368			status.system_enabled = udriver.is_enabled;
369			status.motor1_enabled = udriver.motor1->is_enabled;
370			status.motor1_ready = udriver.motor1->is_ready;
371			status.motor2_enabled = udriver.motor2->is_enabled;
372			status.motor2_ready = udriver.motor2->is_ready;
373			status.error_code = udriver.error_code;
374			status_[i]->append(status);
375			}
376
377			if(!is_ready() \|\| motors_are_paused_)
378			{
379			for(size_t i = 0 ; i < nb_udrivers_ ; ++i)
380			{
381			set_control(i, 0.0, MotorBoardInterface::ControlIndex::current_target_0);
382			set_control(i, 0.0, MotorBoardInterface::ControlIndex::current_target_1);
383			MotorDriver& udriver = main_board_interface_->motor_drivers[i];
384			udriver.motor1->SetCurrentReference(0.0);
385			udriver.motor2->SetCurrentReference(0.0);
386			}
387			main_board_interface_->SendCommand();
388			}
389			spinner.spin();
390			}
391			}
392
393			void SpiBus::wait_until_ready()
394			{
395			while(!is_ready())
396			{
397			rt::Timer::sleep_sec(0.001);
398			}
399			}
400
401	✓✗✓✗	3	} // namespace blmc_drivers