TeensyNanoExoCode
Loading...
Searching...
No Matches
uart_commands.h
Go to the documentation of this file.
1#ifndef UART_COMMANDS_H
2#define UART_COMMANDS_H
3
4#include "Arduino.h"
5#include "UARTHandler.h"
6#include "UART_msg_t.h"
7
8#include "ParseIni.h" // for config info
9#include "ExoData.h"
10#include "JointData.h"
11#include "ParamsFromSD.h"
12#include "Logger.h"
13#include "RealTimeI2C.h"
14
20namespace UART_command_names
21{
22 /* update_x must be get_x + 1 */
23 static const uint8_t empty_msg = 0x00;
24 static const uint8_t get_controller_params = 0x01;
25 static const uint8_t update_controller_params = 0x02;
26 static const uint8_t get_status = 0x03;
27 static const uint8_t update_status = 0x04;
28 static const uint8_t get_config = 0x05;
29 static const uint8_t update_config = 0x06;
30 static const uint8_t get_cal_trq_sensor = 0x07;
31 static const uint8_t update_cal_trq_sensor = 0x08;
32 static const uint8_t get_cal_fsr = 0x09;
33 static const uint8_t update_cal_fsr = 0x0A;
34 static const uint8_t get_refine_fsr = 0x0B;
35 static const uint8_t update_refine_fsr = 0x0C;
36 static const uint8_t get_motor_enable_disable = 0x0D;
37 static const uint8_t update_motor_enable_disable = 0x0E;
38 static const uint8_t get_motor_zero = 0x0F;
39 static const uint8_t update_motor_zero = 0x10;
40 static const uint8_t get_real_time_data = 0x11;
41 static const uint8_t update_real_time_data = 0x12;
42 static const uint8_t get_controller_param = 0x13;
43 static const uint8_t update_controller_param = 0x14;
44 static const uint8_t get_error_code = 0x15;
45 static const uint8_t update_error_code = 0x16;
46 static const uint8_t get_FSR_thesholds = 0x17;
47 static const uint8_t update_FSR_thesholds = 0x18;
48};
49
54namespace UART_command_enums
55{
56 enum class controller_params:uint8_t {
57 CONTROLLER_ID = 0,
58 PARAM_LENGTH = 1,
59 PARAM_START = 2,
60 LENGTH
61 };
62 enum class status:uint8_t {
63 STATUS = 0,
64 LENGTH
65 };
66 enum class cal_trq_sensor:uint8_t {
67 CAL_TRQ_SENSOR = 0,
68 LENGTH
69 };
70 enum class cal_fsr:uint8_t {
71 CAL_FSR = 0,
72 LENGTH
73 };
74 enum class refine_fsr:uint8_t {
75 REFINE_FSR = 0,
76 LENGTH
77 };
78 enum class motor_enable_disable:uint8_t {
79 ENABLE_DISABLE = 0,
80 LENGTH
81 };
82 enum class motor_zero:uint8_t {
83 ZERO = 0,
84 LENGTH
85 };
86 enum class controller_param:uint8_t {
87 CONTROLLER_ID = 0,
88 PARAM_INDEX = 1,
89 PARAM_VALUE = 2,
90 LENGTH
91 };
92 enum class real_time_data:uint8_t {
93
94 };
95 enum class error_code:uint8_t {
96 ERROR_CODE = 0,
97 LENGTH
98 };
99 enum class FSR_thresholds:uint8_t {
100 LEFT_THRESHOLD = 0,
101 RIGHT_THRESHOLD = 1,
102 LENGTH
103 };
104};
105
106
113namespace UART_command_handlers
114{
115 inline static void get_controller_params(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
116 {
117 //logger::println("UART_command_handlers::update_controller_params->Fetching params with msg: ");
118 //UART_msg_t_utils::print_msg(msg);
119
120 JointData* j_data = exo_data->get_joint_with(msg.joint_id);
121 if (j_data == NULL)
122 {
123 logger::println("UART_command_handlers::get_controller_params->No joint with id = "); logger::print(msg.joint_id); logger::println(" found");
124 return;
125 }
126
127 msg.command = UART_command_names::update_controller_params;
128
129 uint8_t param_length = j_data->controller.get_parameter_length();
130 msg.len = param_length + (uint8_t)UART_command_enums::controller_params::LENGTH;
131 msg.data[(uint8_t)UART_command_enums::controller_params::CONTROLLER_ID] = j_data->controller.controller;
132 msg.data[(uint8_t)UART_command_enums::controller_params::PARAM_LENGTH] = param_length;
133 for (int i=0; i<param_length; i++)
134 {
135 msg.data[(uint8_t)UART_command_enums::controller_params::PARAM_START + i] = j_data->controller.parameters[i];
136 }
137
138 handler->UART_msg(msg);
139 }
140 inline static void update_controller_params(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
141 {
142 //TODO: Error checking (valid controller for joint, and matching param length)
143 //logger::println("UART_command_handlers::update_controller_params->Got new params with msg: ");
144 //UART_msg_t_utils::print_msg(msg);
145
146 JointData* j_data = exo_data->get_joint_with(msg.joint_id);
147 if (j_data == NULL)
148 {
149 logger::println("UART_command_handlers::update_controller_params->No joint with id = " + String(msg.joint_id) + " found");
150 return;
151 }
152
153 #if defined(ARDUINO_TEENSY36) || defined(ARDUINO_TEENSY41)
154 j_data->controller.controller = (uint8_t)msg.data[(uint8_t)UART_command_enums::controller_params::CONTROLLER_ID];
155 set_controller_params(msg.joint_id, (uint8_t)msg.data[(uint8_t)UART_command_enums::controller_params::CONTROLLER_ID], (uint8_t)msg.data[(uint8_t)UART_command_enums::controller_params::PARAM_START], exo_data);
156
157 //Serial.println("Updating Controller Params: " + String(msg.joint_id) + ", " + String((uint8_t)msg.data[(uint8_t)UART_command_enums::controller_params::PARAM_START]) + ", " + String(j_data->controller.controller));
158 #endif
159 }
160
161 inline static void get_status(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
162 {
163
164 UART_msg_t tx_msg;
165 tx_msg.command = UART_command_names::update_status;
166 tx_msg.joint_id = 0;
167 tx_msg.len = (uint8_t)UART_command_enums::status::LENGTH;
168 tx_msg.data[(uint8_t)UART_command_enums::status::STATUS] = exo_data->get_status();
169
170 handler->UART_msg(tx_msg);
171 //logger::println("UART_command_handlers::get_status->sent updated status");
172 }
173
174 inline static void update_status(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
175 {
176 exo_data->set_status(msg.data[(uint8_t)UART_command_enums::status::STATUS]);
177 #if defined(ARDUINO_TEENSY36) || defined(ARDUINO_TEENSY41)
178 if (msg.data[(uint8_t)UART_command_enums::status::STATUS] == status_defs::messages::trial_on)
179 {
180 // Set default parameters for each used joint
181 exo_data->set_default_parameters();
182 }
183 #endif
184 }
185
186 inline static void get_config(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
187 {
188 UART_msg_t tx_msg;
189 tx_msg.command = UART_command_names::update_config;
190 tx_msg.joint_id = 0;
191 tx_msg.len = ini_config::number_of_keys;
192 tx_msg.data[config_defs::board_name_idx] = exo_data->config[config_defs::board_name_idx];
193 tx_msg.data[config_defs::battery_idx] = exo_data->config[config_defs::battery_idx];
194 tx_msg.data[config_defs::board_version_idx] = exo_data->config[config_defs::board_version_idx];
195 tx_msg.data[config_defs::exo_name_idx] = exo_data->config[config_defs::exo_name_idx];
196 tx_msg.data[config_defs::exo_side_idx] = exo_data->config[config_defs::exo_side_idx];
197 tx_msg.data[config_defs::hip_idx] = exo_data->config[config_defs::hip_idx];
198 tx_msg.data[config_defs::knee_idx] = exo_data->config[config_defs::knee_idx];
199 tx_msg.data[config_defs::ankle_idx] = exo_data->config[config_defs::ankle_idx];
200 tx_msg.data[config_defs::hip_gear_idx] = exo_data->config[config_defs::hip_gear_idx];
201 tx_msg.data[config_defs::knee_gear_idx] = exo_data->config[config_defs::knee_gear_idx];
202 tx_msg.data[config_defs::ankle_gear_idx] = exo_data->config[config_defs::ankle_gear_idx];
203 tx_msg.data[config_defs::exo_hip_default_controller_idx] = exo_data->config[config_defs::exo_hip_default_controller_idx];
204 tx_msg.data[config_defs::exo_knee_default_controller_idx] = exo_data->config[config_defs::exo_knee_default_controller_idx];
205 tx_msg.data[config_defs::exo_ankle_default_controller_idx] = exo_data->config[config_defs::exo_ankle_default_controller_idx];
206 tx_msg.data[config_defs::hip_flip_dir_idx] = exo_data->config[config_defs::hip_flip_dir_idx];
207 tx_msg.data[config_defs::knee_flip_dir_idx] = exo_data->config[config_defs::knee_flip_dir_idx];
208 tx_msg.data[config_defs::ankle_flip_dir_idx] = exo_data->config[config_defs::ankle_flip_dir_idx];
209
210 handler->UART_msg(tx_msg);
211 //logger::println("UART_command_handlers::get_config->sent updated config");
212 }
213 inline static void update_config(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
214 {
215 //logger::println("UART_command_handlers::update_config->got message: ");
216 UART_msg_t_utils::print_msg(msg);
217 exo_data->config[config_defs::board_name_idx] = msg.data[config_defs::board_name_idx];
218 exo_data->config[config_defs::battery_idx] = msg.data[config_defs::battery_idx];
219 exo_data->config[config_defs::board_version_idx] = msg.data[config_defs::board_version_idx];
220 exo_data->config[config_defs::exo_name_idx] = msg.data[config_defs::exo_name_idx];
221 exo_data->config[config_defs::exo_side_idx] = msg.data[config_defs::exo_side_idx];
222 exo_data->config[config_defs::hip_idx] = msg.data[config_defs::hip_idx];
223 exo_data->config[config_defs::knee_idx] = msg.data[config_defs::knee_idx];
224 exo_data->config[config_defs::ankle_idx] = msg.data[config_defs::ankle_idx];
225 exo_data->config[config_defs::hip_gear_idx] = msg.data[config_defs::hip_gear_idx];
226 exo_data->config[config_defs::knee_gear_idx] = msg.data[config_defs::knee_gear_idx];
227 exo_data->config[config_defs::ankle_gear_idx] = msg.data[config_defs::ankle_gear_idx];
228 exo_data->config[config_defs::exo_hip_default_controller_idx] = msg.data[config_defs::exo_hip_default_controller_idx];
229 exo_data->config[config_defs::exo_knee_default_controller_idx] = msg.data[config_defs::exo_knee_default_controller_idx];
230 exo_data->config[config_defs::exo_ankle_default_controller_idx] = msg.data[config_defs::exo_ankle_default_controller_idx];
231 exo_data->config[config_defs::hip_flip_dir_idx] = msg.data[config_defs::hip_flip_dir_idx];
232 exo_data->config[config_defs::knee_flip_dir_idx] = msg.data[config_defs::knee_flip_dir_idx];
233 exo_data->config[config_defs::ankle_flip_dir_idx] = msg.data[config_defs::ankle_flip_dir_idx];
234 }
235
236 inline static void get_cal_trq_sensor(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
237 {
238
239 }
240 inline static void update_cal_trq_sensor(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
241 {
242 //logger::println("UART_command_handlers::update_cal_trq_sensor->Got Cal trq sensor");
243 exo_data->start_pretrial_cal();
244 }
245
246 inline static void get_cal_fsr(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
247 {
248
249 }
250 inline static void update_cal_fsr(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
251 {
252 //logger::println("UART_command_handlers::update_cal_fsr->Got msg");
253 exo_data->right_leg.do_calibration_toe_fsr = 1;
254 exo_data->right_leg.do_calibration_heel_fsr = 1;
255 exo_data->left_leg.do_calibration_toe_fsr = 1;
256 exo_data->left_leg.do_calibration_heel_fsr = 1;
257 }
258
259 inline static void get_refine_fsr(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
260 {
261
262 }
263 inline static void update_refine_fsr(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
264 {
265 // TODO: only calibrate if the fsr is used
266 //logger::println("UART_command_handlers::update_refine_fsr->Got msg");
267 exo_data->right_leg.do_calibration_refinement_toe_fsr = 1;
268 exo_data->right_leg.do_calibration_refinement_heel_fsr = 1;
269 exo_data->left_leg.do_calibration_refinement_toe_fsr = 1;
270 exo_data->left_leg.do_calibration_refinement_heel_fsr = 1;
271 }
272
273 inline static void get_motor_enable_disable(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
274 {
275
276
277 }
278 inline static void update_motor_enable_disable(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
279 {
280 //logger::println("UART_command_handlers::update_motor_enable_disable->Got msg");
281 exo_data->for_each_joint([](JointData* j_data, float* args) {if (j_data->is_used) j_data->motor.enabled = (bool)args[0];}, msg.data);
282 exo_data->user_paused = !(bool)msg.data[0];
283 }
284
285 inline static void get_motor_zero(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
286 {
287
288 }
289 inline static void update_motor_zero(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
290 {
291
292
293 }
294
295 inline static void get_real_time_data(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg, uint8_t *config)
296 {
297 UART_msg_t rx_msg;
298 rx_msg.command = UART_command_names::update_real_time_data;
299 rx_msg.joint_id = 0;
300 rx_msg.len = (uint8_t)rt_data::BILATERAL_ANKLE_RT_LEN; // TODO: Set based on config
301
302 // logger::println("config[config_defs::exo_name_idx] :: "); //Uncomment if you want to check that system is receiving correct config info
303 // logger::println(config[config_defs::exo_name_idx]);
304
305 switch (config[config_defs::exo_name_idx])
306 {
307 case (uint8_t)config_defs::exo_name::bilateral_ankle:
308 rx_msg.len = (uint8_t)rt_data::BILATERAL_ANKLE_RT_LEN;
309
310 //Jack Plot
311 // rx_msg.data[0] = exo_data->right_leg.percent_stance / 100; // ankle.controller.filtered_torque_reading; //motor.i; //filtered_torque_reading *-1;
312 // rx_msg.data[1] = exo_data->right_leg.ankle.controller.filtered_torque_reading;//exo_data->right_leg.ankle.motor.i;
313 // rx_msg.data[2] = exo_data->right_leg.toe_fsr; // ankle.controller.ff_setpoint;
314 // rx_msg.data[3] = exo_data->left_leg.percent_stance / 100; // ankle.controller.filtered_torque_reading; // filtered_torque_reading; //rx_msg.data[3] = exo_data->right_leg.ankle.motor.i
315 // rx_msg.data[4] = exo_data->left_leg.ankle.controller.filtered_torque_reading;//toe_stance; //(uint8_t) exo_data->right_leg.inclination; //exo_data->left_leg.toe_stance; //exo_data->left_leg.ankle.motor.i;
316 // rx_msg.data[5] = exo_data->left_leg.toe_fsr; // ankle.controller.ff_setpoint;
317 // rx_msg.data[6] = exo_data->right_leg.ankle.controller.ff_setpoint; //toe_fsr; //ankle.joint_position;
318 // rx_msg.data[7] = exo_data->left_leg.ankle.controller.ff_setpoint; //toe_fsr;
319
320 // Chance Plot
321 rx_msg.data[0] = exo_data->right_leg.ankle.controller.filtered_torque_reading;
322 rx_msg.data[1] = exo_data->right_leg.toe_stance;
323 rx_msg.data[2] = exo_data->right_leg.ankle.controller.ff_setpoint;
324 rx_msg.data[3] = exo_data->left_leg.ankle.controller.filtered_torque_reading;
325 rx_msg.data[4] = exo_data->left_leg.toe_stance;
326 rx_msg.data[5] = exo_data->left_leg.ankle.controller.ff_setpoint;
327 rx_msg.data[6] = exo_data->right_leg.toe_fsr;
328 rx_msg.data[7] = exo_data->left_leg.toe_fsr;
329 break;
330
331 case (uint8_t)config_defs::exo_name::bilateral_hip:
332 rx_msg.len = (uint8_t)rt_data::BILATERAL_HIP_RT_LEN;
333 rx_msg.data[0] = exo_data->right_leg.percent_gait / 100;
334 rx_msg.data[1] = exo_data->right_leg.toe_stance;
335 rx_msg.data[2] = exo_data->right_leg.hip.motor.i;// hip.controller.setpoint; //filtered_cmd
336 rx_msg.data[3] = exo_data->left_leg.percent_gait / 100;
337 rx_msg.data[4] = exo_data->left_leg.toe_stance;
338 rx_msg.data[5] = exo_data->left_leg.hip.motor.i;// hip.controller.setpoint; //filtered_cmd
339 rx_msg.data[6] = exo_data->right_leg.toe_fsr;
340 rx_msg.data[7] = exo_data->left_leg.toe_fsr;
341 break;
342
343 case (uint8_t)config_defs::exo_name::bilateral_hip_ankle:
344 rx_msg.len = (uint8_t)rt_data::BILATERAL_ANKLE_RT_LEN;
345 rx_msg.data[0] = exo_data->right_leg.ankle.controller.filtered_torque_reading; //Purple Torque
346 rx_msg.data[1] = exo_data->right_leg.hip.controller.setpoint; //Purple State
347 rx_msg.data[2] = exo_data->right_leg.ankle.controller.ff_setpoint; //Red Torque
348 rx_msg.data[3] = exo_data->left_leg.ankle.controller.filtered_torque_reading; //rx_msg.data[3] = exo_data->right_leg.ankle.motor.i; //Purple Torque
349 //TODO: Implement Mark Feature
350 rx_msg.data[4] = exo_data->left_leg.hip.controller.setpoint; //rx_msg.data[4] = exo_data->left_leg.toe_stance; //Purple State
351 rx_msg.data[5] = exo_data->left_leg.ankle.controller.ff_setpoint; //Red Torque
352 rx_msg.data[6] = exo_data->right_leg.toe_fsr; //Red State
353 rx_msg.data[7] = exo_data->left_leg.toe_fsr; //Red State
354 break;
355
356 case (uint8_t)config_defs::exo_name::right_knee:
357 rx_msg.len = (uint8_t)rt_data::RIGHT_KNEE;
358 rx_msg.data[0] = exo_data->right_leg.knee.controller.filtered_torque_reading;
359 rx_msg.data[1] = exo_data->right_leg.knee.motor.i;//exo_data->right_leg.toe_stance;
360 rx_msg.data[2] = exo_data->right_leg.knee.controller.setpoint;
361 rx_msg.data[3] = exo_data->left_leg.knee.controller.filtered_torque_reading; //rx_msg.data[3] = exo_data->right_leg.ankle.motor.i;
362 //TODO: Implement Mark Feature
363 rx_msg.data[4] = exo_data->left_leg.knee.motor.i;//exo_data->left_leg.toe_stance; //rx_msg.data[4] = exo_data->left_leg.toe_stance;
364 rx_msg.data[5] = exo_data->left_leg.knee.controller.setpoint;
365 //rx_msg.data[6] = exo_data->right_leg.thigh_angle / 100;
366 //rx_msg.data[7] = exo_data->left_leg.thigh_angle / 100;
367 rx_msg.data[6] = exo_data->right_leg.toe_fsr;
368 rx_msg.data[7] = exo_data->left_leg.toe_fsr; //Red State
369 break;
370
371 default:
372 rx_msg.len = (uint8_t)rt_data::BILATERAL_ANKLE_RT_LEN;
373 rx_msg.data[0] = exo_data->right_leg.ankle.controller.filtered_torque_reading;
374 rx_msg.data[1] = exo_data->right_leg.toe_stance;
375 rx_msg.data[2] = exo_data->right_leg.ankle.controller.ff_setpoint;
376 rx_msg.data[3] = exo_data->left_leg.ankle.controller.filtered_torque_reading;
377 //TODO: Implement Mark Feature
378 rx_msg.data[4] = exo_data->left_leg.toe_stance;
379 rx_msg.data[5] = exo_data->left_leg.ankle.controller.ff_setpoint;
380 rx_msg.data[6] = exo_data->right_leg.toe_fsr;
381 rx_msg.data[7] = exo_data->left_leg.toe_fsr;
382 break;
383 }
384
385
386 #if REAL_TIME_I2C
387 real_time_i2c::msg(rx_msg.data, rx_msg.len);
388 #else
389 handler->UART_msg(rx_msg);
390 #endif
391 //logger::println("UART_command_handlers::get_real_time_data->sent real time data"); Uncomment if you want to test to see what data is being sent
392 //UART_msg_t_utils::print_msg(rx_msg);
393 }
394
395 // Overload for no config
396 inline static void get_real_time_data(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
397 {
398 uint8_t empty_config[ini_config::number_of_keys] = {0};
399 get_real_time_data(handler, exo_data, msg, empty_config);
400 }
401
402
403 inline static void update_real_time_data(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
404 {
405 // logger::println("UART_command_handlers::update_real_time_data->got message: ");
406 // UART_msg_t_utils::print_msg(msg);
407 #if REAL_TIME_I2C
408 return;
409 #endif
410 if (rt_data::len != msg.len)
411 {
412 return;
413 }
414 for (int i = 0; i < rt_data::len; i++)
415 {
416 rt_data::float_values[i] = msg.data[i];
417 }
418 rt_data::new_rt_msg = true;
419 // exo_data->right_leg.ankle.torque_reading = msg.data[0];
420 // exo_data->right_leg.ankle.controller.setpoint = msg.data[2];
421 // exo_data->left_leg.ankle.torque_reading = msg.data[3];
422 // exo_data->left_leg.ankle.controller.setpoint = msg.data[5];
423 // exo_data->right_leg.toe_fsr = msg.data[6];
424 // exo_data->left_leg.toe_fsr = msg.data[7];
425 }
426
427 inline static void update_controller_param(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
428 {
429 // Get the joint
430 JointData* j_data = exo_data->get_joint_with(msg.joint_id);
431 if (j_data == NULL)
432 {
433 logger::println("UART_command_handlers::update_controller_param->No joint with id = "); logger::print(msg.joint_id); logger::println(" found");
434 return;
435 }
436
437 // Set the controller
438 if (msg.data[(uint8_t)UART_command_enums::controller_params::CONTROLLER_ID] != j_data->controller.controller)
439 {
440 j_data->controller.controller = (uint8_t)msg.data[(uint8_t)UART_command_enums::controller_params::CONTROLLER_ID];
441 exo_data->set_default_parameters();
442 }
443
444 // Set the parameter
445 j_data->controller.parameters[(uint8_t)msg.data[(uint8_t)UART_command_enums::controller_param::PARAM_INDEX]] = msg.data[(uint8_t)UART_command_enums::controller_param::PARAM_VALUE];
446 // Serial.println("Updating Controller Params: " + String(msg.joint_id) + ", "
447 // + String((uint8_t)msg.data[(uint8_t)UART_command_enums::controller_param::CONTROLLER_ID]) + ", "
448 // + String((uint8_t)msg.data[(uint8_t)UART_command_enums::controller_param::PARAM_INDEX]) + ", "
449 // + String((uint8_t)msg.data[(uint8_t)UART_command_enums::controller_param::PARAM_VALUE]) + ", ");
450 }
451
452 inline static void update_error_code(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
453 {
454 //logger::println("UART_command_handlers::update_error_code->got message: ");
455 //UART_msg_t_utils::print_msg(msg);
456
457 // Set the error code
458 exo_data->error_code = msg.data[0];
459 exo_data->error_joint_id = msg.joint_id;
460 }
461
462 inline static void get_error_code(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
463 {
464 UART_msg_t tx_msg;
465 tx_msg.command = UART_command_names::update_error_code;
466 tx_msg.joint_id = exo_data->error_joint_id;
467 tx_msg.len = 1;
468 tx_msg.data[0] = exo_data->error_code;
469 handler->UART_msg(tx_msg);
470 // logger::println("Sent error code");
471 }
472
473 inline static void get_FSR_thesholds(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
474 {
475 UART_msg_t tx_msg;
476 tx_msg.command = UART_command_names::update_FSR_thesholds;
477 tx_msg.joint_id = 0;
478 tx_msg.len = (uint8_t)UART_command_enums::FSR_thresholds::LENGTH;
479 tx_msg.data[(uint8_t)UART_command_enums::FSR_thresholds::RIGHT_THRESHOLD] =
480 (exo_data->right_leg.toe_fsr_upper_threshold + exo_data->right_leg.toe_fsr_lower_threshold)/2;
481 tx_msg.data[(uint8_t)UART_command_enums::FSR_thresholds::LEFT_THRESHOLD] =
482 (exo_data->left_leg.toe_fsr_upper_threshold + exo_data->left_leg.toe_fsr_lower_threshold)/2;
483 handler->UART_msg(tx_msg);
484 // logger::println("Sent FSR thresholds");
485 }
486 inline static void update_FSR_thesholds(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
487 {
488 // logger::println("UART_command_handlers::update_FSR_thesholds->got message: ");
489 // UART_msg_t_utils::print_msg(msg);
490 exo_data->right_leg.toe_fsr_upper_threshold = msg.data[(uint8_t)UART_command_enums::FSR_thresholds::RIGHT_THRESHOLD] + fsr_config::SCHMITT_DELTA;
491 exo_data->right_leg.toe_fsr_lower_threshold = msg.data[(uint8_t)UART_command_enums::FSR_thresholds::RIGHT_THRESHOLD] - fsr_config::SCHMITT_DELTA;
492 exo_data->left_leg.toe_fsr_upper_threshold = msg.data[(uint8_t)UART_command_enums::FSR_thresholds::LEFT_THRESHOLD] + fsr_config::SCHMITT_DELTA;
493 exo_data->left_leg.toe_fsr_lower_threshold = msg.data[(uint8_t)UART_command_enums::FSR_thresholds::LEFT_THRESHOLD] - fsr_config::SCHMITT_DELTA;
494 }
495};
496
497
498namespace UART_command_utils
499{
500
501 static UART_msg_t call_and_response(UARTHandler* handler, UART_msg_t msg, float timeout)
502 {
503 UART_msg_t rx_msg = {0, 0, 0, 0};
504 uint8_t searching = 1;
505 // logger::println("UART_command_utils::call_and_response->searching for message");
506 float start_time = millis();
507 while (searching)
508 {
509 handler->UART_msg(msg);
510 // logger::println("UART_command_utils::call_and_response->sent msg");
511 delay(500);
512 rx_msg = handler->poll(200000);
513 searching = (rx_msg.command != (msg.command+1));
514 // TODO add timeout
515 if (millis() - start_time > timeout)
516 {
517 // logger::println("UART_command_utils::call_and_response->timed out");
518 return rx_msg;
519 }
520 }
521 // logger::println("UART_command_utils::call_and_response->found message:");
522 UART_msg_t_utils::print_msg(rx_msg);
523 return rx_msg;
524 }
525
526 static uint8_t get_config(UARTHandler* handler, uint8_t* config, float timeout)
527 {
528 UART_msg_t msg;
529 float start_time = millis();
530 while (1)
531 {
532 msg.command = UART_command_names::get_config;
533 msg.len = 0;
534 msg = call_and_response(handler, msg, timeout);
535
536 if ((millis() - start_time) > timeout)
537 {
538 logger::println("UART_command_utils::get_config->timed out");
539 return 1;
540 }
541
542 // the length of the message needs to be equal to the config length
543 if (msg.len != ini_config::number_of_keys)
544 {
545 logger::println("UART_command_utils::get_config->msg.len != number_of_keys");
546 // keep trying to get config
547 continue;
548 }
549 for (int i=0; i<msg.len; i++)
550 {
551 // a valid config will not contain a zero
552 if (!msg.data[i])
553 {
554 logger::print("UART_command_utils::get_config->Config contained a zero at index ");
555 logger::println(i);
556
557 // keep trying to get config
558 continue;
559 }
560 }
561 logger::println("UART_command_utils::get_config->got good config");
562 break;
563 }
564
565 // pack config
566 for (int i=0; i<msg.len; i++)
567 {
568 config[i] = msg.data[i];
569 }
570 return 0;
571 }
572
573 static void wait_for_get_config(UARTHandler* handler, ExoData* data, float timeout)
574 {
575 UART_msg_t rx_msg;
576 float start_time = millis();
577 while (true)
578 {
579 logger::println("UART_command_utils::wait_for_config->Polling for config");
580 rx_msg = handler->poll(100000);
581 if (rx_msg.command == UART_command_names::get_config)
582 {
583 logger::println("UART_command_utils::wait_for_config->Got config request");
584 UART_command_handlers::get_config(handler, data, rx_msg);
585 break;
586 }
587 delayMicroseconds(500);
588
589 if ((millis() - start_time) > timeout)
590 {
591 logger::println("UART_command_utils::wait_for_config->Timed out");
592 return;
593 }
594 }
595 logger::println("UART_command_utils::wait_for_config->Sent config");
596 }
597
598
599
600 static void handle_msg(UARTHandler* handler, ExoData* exo_data, UART_msg_t msg)
601 {
602 if (msg.command == UART_command_names::empty_msg)
603 {
604 return;
605 }
606
607 logger::println("UART_command_utils::handle_message->got message: ");
608 UART_msg_t_utils::print_msg(msg);
609
610 switch (msg.command)
611 {
612 case UART_command_names::empty_msg:
613 logger::println("UART_command_utils::handle_message->Empty Message!");
614 break;
615
616 case UART_command_names::get_controller_params:
617 UART_command_handlers::get_controller_params(handler, exo_data, msg);
618 break;
619 case UART_command_names::update_controller_params:
620 UART_command_handlers::update_controller_params(handler, exo_data, msg);
621 break;
622
623 case UART_command_names::get_status:
624 UART_command_handlers::get_status(handler, exo_data, msg);
625 break;
626 case UART_command_names::update_status:
627 UART_command_handlers::update_status(handler, exo_data, msg);
628 break;
629
630 case UART_command_names::get_config:
631 UART_command_handlers::get_config(handler, exo_data, msg);
632 break;
633 case UART_command_names::update_config:
634 UART_command_handlers::update_config(handler, exo_data, msg);
635 break;
636
637 case UART_command_names::get_cal_trq_sensor:
638 UART_command_handlers::get_cal_trq_sensor(handler, exo_data, msg);
639 break;
640 case UART_command_names::update_cal_trq_sensor:
641 UART_command_handlers::update_cal_trq_sensor(handler, exo_data, msg);
642 break;
643
644 case UART_command_names::get_cal_fsr:
645 UART_command_handlers::get_cal_fsr(handler, exo_data, msg);
646 break;
647 case UART_command_names::update_cal_fsr:
648 UART_command_handlers::update_cal_fsr(handler, exo_data, msg);
649 break;
650
651 case UART_command_names::get_refine_fsr:
652 UART_command_handlers::get_refine_fsr(handler, exo_data, msg);
653 break;
654 case UART_command_names::update_refine_fsr:
655 UART_command_handlers::update_refine_fsr(handler, exo_data, msg);
656 break;
657
658 case UART_command_names::get_motor_enable_disable:
659 UART_command_handlers::get_motor_enable_disable(handler, exo_data, msg);
660 break;
661 case UART_command_names::update_motor_enable_disable:
662 UART_command_handlers::update_motor_enable_disable(handler, exo_data, msg);
663 break;
664
665 case UART_command_names::get_motor_zero:
666 UART_command_handlers::get_motor_zero(handler, exo_data, msg);
667 break;
668 case UART_command_names::update_motor_zero:
669 UART_command_handlers::update_motor_zero(handler, exo_data, msg);
670 break;
671
672 case UART_command_names::get_real_time_data:
673 UART_command_handlers::get_real_time_data(handler, exo_data, msg);
674 break;
675 case UART_command_names::update_real_time_data:
676 UART_command_handlers::update_real_time_data(handler, exo_data, msg);
677 break;
678
679 case UART_command_names::update_controller_param:
680 UART_command_handlers::update_controller_param(handler, exo_data, msg);
681 break;
682
683 case UART_command_names::get_error_code:
684 UART_command_handlers::get_error_code(handler, exo_data, msg);
685 break;
686 case UART_command_names::update_error_code:
687 UART_command_handlers::update_error_code(handler, exo_data, msg);
688 break;
689
690 case UART_command_names::get_FSR_thesholds:
691 UART_command_handlers::get_FSR_thesholds(handler, exo_data, msg);
692 break;
693 case UART_command_names::update_FSR_thesholds:
694 UART_command_handlers::update_FSR_thesholds(handler, exo_data, msg);
695 break;
696
697
698 default:
699 logger::println("UART_command_utils::handle_message->Unknown Message!", LogLevel::Error);
700 UART_msg_t_utils::print_msg(msg);
701 break;
702 }
703 }
704};
705
706
707
708
709
710
711#endif
ExoData.h
Declares a class used to store data for the Exo to access.
JointData.h
Declares a class used to store data for joint to access.
ParamsFromSD.h
Declares the functions to pull controller parameters from the SD card and defines the mapping to the ...
ParseIni.h
Declares the functions needed and defines mapping between the INI keys and the exo components.
UART_msg_t.h
UARTHandler.h
Singleton class that manages the UART data. NOT THREAD SAFE. The class queues recieved messages.
ControllerData::setpoint
float setpoint
Definition ControllerData.h:323
ControllerData::controller
uint8_t controller
Definition ControllerData.h:320
ControllerData::ff_setpoint
float ff_setpoint
Definition ControllerData.h:324
ControllerData::get_parameter_length
uint8_t get_parameter_length()
Get the parameter length for the current controller.
Definition ControllerData.cpp:80
ControllerData::parameters
float parameters[controller_defs::max_parameters]
Definition ControllerData.h:325
ControllerData::filtered_torque_reading
float filtered_torque_reading
Definition ControllerData.h:328
ExoData
Class to store all the data related to the exo.
Definition ExoData.h:38
ExoData::set_default_parameters
void set_default_parameters()
Set the default controller parameters for the current controller. These are the first row in the cont...
Definition ExoData.cpp:103
ExoData::for_each_joint
void for_each_joint(F &&func)
performs a function for each joint
Definition ExoData.h:55
ExoData::user_paused
bool user_paused
Definition ExoData.h:131
ExoData::right_leg
LegData right_leg
Definition ExoData.h:122
ExoData::error_joint_id
int error_joint_id
Definition ExoData.h:130
ExoData::error_code
int error_code
Definition ExoData.h:129
ExoData::get_joint_with
JointData * get_joint_with(uint8_t id)
Get the joint pointer for a joint id.
Definition ExoData.cpp:56
ExoData::get_status
uint16_t get_status(void)
Get the status object.
Definition ExoData.cpp:98
ExoData::start_pretrial_cal
void start_pretrial_cal()
Start the pretrial calibration process.
Definition ExoData.cpp:116
ExoData::config
uint8_t * config
Definition ExoData.h:126
ExoData::left_leg
LegData left_leg
Definition ExoData.h:121
ExoData::set_status
void set_status(uint16_t status_to_set)
Set the status object.
Definition ExoData.cpp:88
JointData
class to store information related to joint.
Definition JointData.h:33
JointData::is_used
bool is_used
Definition JointData.h:50
JointData::controller
ControllerData controller
Definition JointData.h:46
JointData::motor
MotorData motor
Definition JointData.h:45
LegData::do_calibration_refinement_toe_fsr
bool do_calibration_refinement_toe_fsr
Definition LegData.h:76
LegData::toe_stance
bool toe_stance
Definition LegData.h:69
LegData::do_calibration_toe_fsr
bool do_calibration_toe_fsr
Definition LegData.h:75
LegData::toe_fsr_upper_threshold
float toe_fsr_upper_threshold
Definition LegData.h:60
LegData::knee
JointData knee
Definition LegData.h:43
LegData::do_calibration_heel_fsr
bool do_calibration_heel_fsr
Definition LegData.h:77
LegData::ankle
JointData ankle
Definition LegData.h:44
LegData::hip
JointData hip
Definition LegData.h:42
LegData::toe_fsr_lower_threshold
float toe_fsr_lower_threshold
Definition LegData.h:61
LegData::toe_fsr
float toe_fsr
Definition LegData.h:59
LegData::do_calibration_refinement_heel_fsr
bool do_calibration_refinement_heel_fsr
Definition LegData.h:78
LegData::percent_gait
float percent_gait
Definition LegData.h:47
MotorData::i
float i
Definition MotorData.h:41
MotorData::enabled
bool enabled
Definition MotorData.h:50
UARTHandler
Singleton Class to handle the UART Work.
Definition UARTHandler.h:54
UARTHandler::UART_msg
void UART_msg(uint8_t msg_id, uint8_t len, uint8_t joint_id, float *buffer)
Packs and sends a UART message.
Definition UARTHandler.cpp:35
UARTHandler::poll
UART_msg_t poll(float timeout_us=RX_TIMEOUT_US)
Check for incoming data. If there is data read the message, timing out if it takes too long.
Definition UARTHandler.cpp:73
UART_command_enums
Holds all of the enums for the UART commands. The enums are used to properly index the data.
Definition uart_commands.h:55
UART_command_enums::FSR_thresholds
FSR_thresholds
Definition uart_commands.h:99
UART_command_enums::FSR_thresholds::LEFT_THRESHOLD
@ LEFT_THRESHOLD
UART_command_enums::FSR_thresholds::LENGTH
@ LENGTH
UART_command_enums::FSR_thresholds::RIGHT_THRESHOLD
@ RIGHT_THRESHOLD
UART_command_enums::cal_trq_sensor
cal_trq_sensor
Definition uart_commands.h:66
UART_command_enums::cal_trq_sensor::CAL_TRQ_SENSOR
@ CAL_TRQ_SENSOR
UART_command_enums::motor_zero
motor_zero
Definition uart_commands.h:82
UART_command_enums::motor_zero::ZERO
@ ZERO
UART_command_enums::motor_enable_disable
motor_enable_disable
Definition uart_commands.h:78
UART_command_enums::motor_enable_disable::ENABLE_DISABLE
@ ENABLE_DISABLE
UART_command_enums::error_code
error_code
Definition uart_commands.h:95
UART_command_enums::error_code::ERROR_CODE
@ ERROR_CODE
UART_command_enums::controller_params
controller_params
Definition uart_commands.h:56
UART_command_enums::controller_params::PARAM_START
@ PARAM_START
UART_command_enums::controller_params::PARAM_LENGTH
@ PARAM_LENGTH
UART_command_enums::controller_params::CONTROLLER_ID
@ CONTROLLER_ID
UART_command_enums::controller_params::LENGTH
@ LENGTH
UART_command_enums::controller_param
controller_param
Definition uart_commands.h:86
UART_command_enums::controller_param::PARAM_INDEX
@ PARAM_INDEX
UART_command_enums::controller_param::PARAM_VALUE
@ PARAM_VALUE
UART_command_enums::real_time_data
real_time_data
Definition uart_commands.h:92
UART_command_enums::cal_fsr
cal_fsr
Definition uart_commands.h:70
UART_command_enums::cal_fsr::CAL_FSR
@ CAL_FSR
UART_command_enums::status
status
Definition uart_commands.h:62
UART_command_enums::status::STATUS
@ STATUS
UART_command_enums::status::LENGTH
@ LENGTH
UART_command_enums::refine_fsr
refine_fsr
Definition uart_commands.h:74
UART_command_enums::refine_fsr::REFINE_FSR
@ REFINE_FSR
UART_command_handlers
Holds the handlers for all of the commands. The handler function types should be the same....
Definition uart_commands.h:114
UART_command_names
Type to associate a command with an ammount of data.
Definition uart_commands.h:21
UART_command_utils
Definition uart_commands.h:499
config_defs::exo_name::bilateral_hip_ankle
@ bilateral_hip_ankle
config_defs::exo_name::right_knee
@ right_knee
config_defs::exo_name::bilateral_ankle
@ bilateral_ankle
config_defs::exo_name::bilateral_hip
@ bilateral_hip
fsr_config::SCHMITT_DELTA
const float SCHMITT_DELTA
Definition Config.h:43
ini_config::number_of_keys
const int number_of_keys
Definition ParseIni.h:25
real_time_i2c::msg
void msg(float *data, int len)
Definition RealTimeI2C.cpp:55
status_defs::messages::trial_on
const uint16_t trial_on
Definition StatusDefs.h:31
UART_msg_t
Definition UART_msg_t.h:9
UART_msg_t::data
float data[UART_MSG_T_MAX_DATA_LEN]
Definition UART_msg_t.h:12
UART_msg_t::len
uint8_t len
Definition UART_msg_t.h:13
UART_msg_t::joint_id
uint8_t joint_id
Definition UART_msg_t.h:11
UART_msg_t::command
uint8_t command
Definition UART_msg_t.h:10
Generated by doxygen 1.9.7