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 |