diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 4e716925..f0dee75c 100755 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -35,7 +35,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct #define ENCODER_CPR (600*4) #define POLE_PAIRS 7 -const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); +float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public @@ -219,100 +219,6 @@ static const int current_meas_hz = CURRENT_MEAS_HZ; /* Private variables ---------------------------------------------------------*/ static float brake_resistance = 0.47f; // [ohm] - -/* variables exposed to usb interface via set/get/monitor - * If you change something here, don't forget to regenerate the python interface with generate_api.py - * ro/rw : read only/read write -> ro prevents the code generator from generating setter - * */ - -float* exposed_floats[] = { - &motors[0].pos_setpoint, // rw - &motors[0].pos_gain, // rw - &motors[0].vel_setpoint, // rw - &motors[0].vel_gain, // rw - &motors[0].vel_integrator_gain, // rw - &motors[0].vel_integrator_current, // rw - &motors[0].vel_limit, // rw - &motors[0].current_setpoint, // rw - &motors[0].calibration_current, // rw - &motors[0].phase_inductance, // ro - &motors[0].phase_resistance, // ro - &motors[0].current_meas.phB, // ro - &motors[0].current_meas.phC, // ro - &motors[0].DC_calib.phB, // rw - &motors[0].DC_calib.phC, // rw - &motors[0].shunt_conductance, // rw - &motors[0].phase_current_rev_gain, // rw - &motors[0].current_control.current_lim, // rw - &motors[0].current_control.p_gain, // rw - &motors[0].current_control.i_gain, // rw - &motors[0].current_control.v_current_control_integral_d, // rw - &motors[0].current_control.v_current_control_integral_q, // rw - &motors[0].current_control.Ibus, // ro - &motors[0].encoder.phase, // ro - &motors[0].encoder.pll_pos, // rw - &motors[0].encoder.pll_vel, // rw - &motors[0].encoder.pll_kp, // rw - &motors[0].encoder.pll_ki, // rw - &motors[1].pos_setpoint, // rw - &motors[1].pos_gain, // rw - &motors[1].vel_setpoint, // rw - &motors[1].vel_gain, // rw - &motors[1].vel_integrator_gain, // rw - &motors[1].vel_integrator_current, // rw - &motors[1].vel_limit, // rw - &motors[1].current_setpoint, // rw - &motors[1].calibration_current, // rw - &motors[1].phase_inductance, // ro - &motors[1].phase_resistance, // ro - &motors[1].current_meas.phB, // ro - &motors[1].current_meas.phC, // ro - &motors[1].DC_calib.phB, // rw - &motors[1].DC_calib.phC, // rw - &motors[1].shunt_conductance, // rw - &motors[1].phase_current_rev_gain, // rw - &motors[1].current_control.current_lim, // rw - &motors[1].current_control.p_gain, // rw - &motors[1].current_control.i_gain, // rw - &motors[1].current_control.v_current_control_integral_d, // rw - &motors[1].current_control.v_current_control_integral_q, // rw - &motors[1].current_control.Ibus, // ro - &motors[1].encoder.phase, // ro - &motors[1].encoder.pll_pos, // rw - &motors[1].encoder.pll_vel, // rw - &motors[1].encoder.pll_kp, // rw - &motors[1].encoder.pll_ki, // rw -}; - -int* exposed_ints[] = { - (int*)&motors[0].control_mode, // rw - &motors[0].encoder.encoder_offset, // rw - &motors[0].encoder.encoder_state, // ro - &motors[0].error, // rw - (int*)&motors[1].control_mode, // rw - &motors[1].encoder.encoder_offset, // rw - &motors[1].encoder.encoder_state, // ro - &motors[1].error, // rw -}; - -bool* exposed_bools[] = { - &motors[0].thread_ready, // ro - NULL, // rw - NULL, // rw - NULL, // ro - &motors[1].thread_ready, // ro - NULL, // rw - NULL, // rw - NULL, // ro -}; - -uint16_t* exposed_uint16[] = { - &motors[0].control_deadline, // rw - &motors[0].last_cpu_time, // ro - &motors[1].control_deadline, // rw - &motors[1].last_cpu_time, // ro -}; - /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h index f05d1bd4..853d0af0 100644 --- a/MotorControl/low_level.h +++ b/MotorControl/low_level.h @@ -155,7 +155,7 @@ typedef struct{ extern float vbus_voltage; extern Motor_t motors[]; extern const size_t num_motors; -extern const float elec_rad_per_enc; +extern float elec_rad_per_enc; // TODO: make this const (once exposed_floats is gone) /* Exported variables --------------------------------------------------------*/ // Exposed comms table during refactor transition diff --git a/MotorControl/protocol.cpp b/MotorControl/protocol.cpp index 8aaa9166..9fce2f15 100644 --- a/MotorControl/protocol.cpp +++ b/MotorControl/protocol.cpp @@ -1,25 +1,199 @@ +/* Includes ------------------------------------------------------------------*/ + #include "low_level.h" #include "protocol.h" #include +/* Private defines -----------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ + +/* variables exposed to usb interface via set/get/monitor + * If you change something here, don't forget to regenerate the python interface with generate_api.py + * ro/rw : read only/read write -> ro prevents the code generator from generating setter + * */ + +float* exposed_floats[] = { + &vbus_voltage, // ro + &elec_rad_per_enc, // ro + &motors[0].pos_setpoint, // rw + &motors[0].pos_gain, // rw + &motors[0].vel_setpoint, // rw + &motors[0].vel_gain, // rw + &motors[0].vel_integrator_gain, // rw + &motors[0].vel_integrator_current, // rw + &motors[0].vel_limit, // rw + &motors[0].current_setpoint, // rw + &motors[0].calibration_current, // rw + &motors[0].phase_inductance, // ro + &motors[0].phase_resistance, // ro + &motors[0].current_meas.phB, // ro + &motors[0].current_meas.phC, // ro + &motors[0].DC_calib.phB, // rw + &motors[0].DC_calib.phC, // rw + &motors[0].shunt_conductance, // rw + &motors[0].phase_current_rev_gain, // rw + &motors[0].current_control.current_lim, // rw + &motors[0].current_control.p_gain, // rw + &motors[0].current_control.i_gain, // rw + &motors[0].current_control.v_current_control_integral_d, // rw + &motors[0].current_control.v_current_control_integral_q, // rw + &motors[0].current_control.Ibus, // ro + &motors[0].encoder.phase, // ro + &motors[0].encoder.pll_pos, // rw + &motors[0].encoder.pll_vel, // rw + &motors[0].encoder.pll_kp, // rw + &motors[0].encoder.pll_ki, // rw + &motors[1].pos_setpoint, // rw + &motors[1].pos_gain, // rw + &motors[1].vel_setpoint, // rw + &motors[1].vel_gain, // rw + &motors[1].vel_integrator_gain, // rw + &motors[1].vel_integrator_current, // rw + &motors[1].vel_limit, // rw + &motors[1].current_setpoint, // rw + &motors[1].calibration_current, // rw + &motors[1].phase_inductance, // ro + &motors[1].phase_resistance, // ro + &motors[1].current_meas.phB, // ro + &motors[1].current_meas.phC, // ro + &motors[1].DC_calib.phB, // rw + &motors[1].DC_calib.phC, // rw + &motors[1].shunt_conductance, // rw + &motors[1].phase_current_rev_gain, // rw + &motors[1].current_control.current_lim, // rw + &motors[1].current_control.p_gain, // rw + &motors[1].current_control.i_gain, // rw + &motors[1].current_control.v_current_control_integral_d, // rw + &motors[1].current_control.v_current_control_integral_q, // rw + &motors[1].current_control.Ibus, // ro + &motors[1].encoder.phase, // ro + &motors[1].encoder.pll_pos, // rw + &motors[1].encoder.pll_vel, // rw + &motors[1].encoder.pll_kp, // rw + &motors[1].encoder.pll_ki, // rw +}; + +int* exposed_ints[] = { + (int*)&motors[0].control_mode, // rw + &motors[0].encoder.encoder_offset, // rw + &motors[0].encoder.encoder_state, // ro + &motors[0].error, // rw + (int*)&motors[1].control_mode, // rw + &motors[1].encoder.encoder_offset, // rw + &motors[1].encoder.encoder_state, // ro + &motors[1].error, // rw +}; + +bool* exposed_bools[] = { + &motors[0].thread_ready, // ro + NULL, // rw + NULL, // rw + NULL, // ro + &motors[1].thread_ready, // ro + NULL, // rw + NULL, // rw + NULL, // ro +}; + +uint16_t* exposed_uint16[] = { + &motors[0].control_deadline, // rw + &motors[0].last_cpu_time, // ro + &motors[1].control_deadline, // rw + &motors[1].last_cpu_time, // ro +}; + +/* Monitoring */ +size_t monitoring_slots[20] = {0}; +constexpr size_t NUM_MONITORING_SLOTS = sizeof(monitoring_slots) / sizeof(monitoring_slots[0]); + + +/* Variables exposed to USB & UART via read/write commands */ +// TODO: include range information in JSON description + +// clang-format off Endpoint endpoints[] = { Endpoint("vbus_voltage", static_cast(vbus_voltage)), - Endpoint("elec_rad_per_enc", elec_rad_per_enc), + Endpoint("elec_rad_per_enc", static_cast(elec_rad_per_enc)), Endpoint("motor0", BEGIN_TREE, nullptr, nullptr, nullptr), Endpoint("pos_setpoint", motors[0].pos_setpoint), Endpoint("pos_gain", motors[0].pos_gain), Endpoint("vel_setpoint", motors[0].vel_setpoint), Endpoint(nullptr, END_TREE, nullptr, nullptr, nullptr) // motor0 }; +// clang-format on constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); -/* Monitoring */ -size_t monitoring_slots[20] = {0}; -constexpr size_t NUM_MONITORING_SLOTS = sizeof(monitoring_slots) / sizeof(monitoring_slots[0]); +/* Private function prototypes -----------------------------------------------*/ + +int Protocol_print_simple_type(TypeInfo_t type_info, const void *ctx); +int Protocol_scan_simple_type(TypeInfo_t type_info, const char* buffer, void *ctx); +void Protocol_print_json(void); +void Protocol_print_monitoring(size_t limit); +void Protocol_parse_cmd(uint8_t* buffer, int len); + +/* Function implementations --------------------------------------------------*/ + +void Endpoint::print_json(size_t id, bool& need_comma) { + if (type_info_ == END_TREE) { + printf("]}"); + need_comma = true; + return; + } else if (type_info_ < END_TREE) { + if (need_comma) + printf(","); + printf("{\"name\":\"%s\",\"id\":%u,\"type\":\"%s\"", + name_ ? name_ : "", id, + type_names_[type_info_] ? type_names_[type_info_] : ""); + if (type_info_ == BEGIN_TREE) { + printf(",\"content\":["); + need_comma = false; + } else { + printf(",\"access\":\""); + if (print_callback_) + printf("r"); + if (scan_callback_) + printf("w"); + printf("\"}"); + need_comma = true; + } + } +} + +// Returns 0 on success, otherwise a non-zero error code +int Protocol_print_simple_type(TypeInfo_t type_info, const void *ctx) { + switch (type_info) { + case AS_FLOAT: + printf("%f", *reinterpret_cast(ctx)); break; + case AS_INT: printf("%d", *reinterpret_cast(ctx)); break; + case AS_BOOL: printf("%d", *reinterpret_cast(ctx)); break; + case AS_UINT16: printf("%hu", *reinterpret_cast(ctx)); break; + default: + return -1; + } + return 0; +} + +// Returns 0 on success, otherwise a non-zero error code +int Protocol_scan_simple_type(TypeInfo_t type_info, const char* buffer, void *ctx) { + switch (type_info) { + case AS_FLOAT: sscanf(buffer, "%f", reinterpret_cast(ctx)); break; + case AS_INT: sscanf(buffer, "%d", reinterpret_cast(ctx)); break; + case AS_BOOL: sscanf(buffer, "%d", reinterpret_cast(ctx)); break; + case AS_UINT16: sscanf(buffer, "%hu", reinterpret_cast(ctx)); break; + default: + return -1; + } + return 0; +} void Protocol_print_json(void) { bool need_comma = false; @@ -78,8 +252,7 @@ void Protocol_parse_cmd(uint8_t* buffer, int len) { // s index value size_t index = 0; size_t pos = 0; - // Oskar: format string start should be w, not s - int numscan = sscanf((const char*)buffer, "s %u %n", &index, &pos); + int numscan = sscanf((const char*)buffer, "w %u %n", &index, &pos); if (numscan == 1) { if (index < NUM_ENDPOINTS) { endpoints[index].scan_value((const char*)buffer + pos); @@ -94,6 +267,58 @@ void Protocol_parse_cmd(uint8_t* buffer, int len) { endpoints[index].print_value(); } } + } else if (buffer[0] == 'g') { // GET + // g <0:float,1:int,2:bool,3:uint16> index + int type = 0; + int index = 0; + int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index); + if (numscan == 2) { + switch(type){ + case 0: { + printf("%f\n",*exposed_floats[index]); + break; + }; + case 1: { + printf("%d\n",*exposed_ints[index]); + break; + }; + case 2: { + printf("%d\n",*exposed_bools[index]); + break; + }; + case 3: { + printf("%hu\n",*exposed_uint16[index]); + break; + }; + } + } + } else if (buffer[0] == 's') { // SET + // s <0:float,1:int,2:bool,3:uint16> index value + int type = 0; + int index = 0; + int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index); + if (numscan == 2) { + switch(type) { + case 0: { + sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]); + break; + }; + case 1: { + sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]); + break; + }; + case 2: { + int btmp = 0; + sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp); + *exposed_bools[index] = btmp ? true : false; + break; + }; + case 3: { + sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]); + break; + }; + } + } } else if (buffer[0] == 'm') { // Setup Monitor // m index monitoring_slot size_t index = 0; diff --git a/MotorControl/protocol.h b/MotorControl/protocol.h index 9310aa13..cf627c6e 100644 --- a/MotorControl/protocol.h +++ b/MotorControl/protocol.h @@ -2,14 +2,10 @@ // TODO: resolve assert #define assert(expr) - #ifdef __cplusplus #include -typedef std::function PrintCallback; -typedef std::function ScanCallback; - typedef enum { AS_FLOAT, AS_INT, @@ -19,8 +15,11 @@ typedef enum { END_TREE } TypeInfo_t; +typedef std::function PrintCallback; +typedef std::function ScanCallback; + // The order in this list must correspond to the order in TypeInfo_t -const char *_type_names[] = { +const char *type_names_[] = { "float", "int", "bool", @@ -29,113 +28,70 @@ const char *_type_names[] = { }; +int Protocol_print_simple_type(TypeInfo_t type_info, const void *ctx); +int Protocol_scan_simple_type(TypeInfo_t type_info, const char* buffer, void *ctx); + // Default getters/setters -// Oskar: Instead of creating all these functions, couldn't you just create one -// function called "default printer", which takes as an argument the _type_info, -// and has a big switch statement? - -// You could extend this, and store only a single parse function pointer, and -// pass in a bool indicating if we are reading or writing, hence save some memory. -// We lose generality of nulling out the scanf part on const, but you could just -// create duplicates in TypeInfo_t (const and non-const). Debatable if it's worth it... - -PrintCallback print_float = std::bind(printf, "%f", std::placeholders::_1); -ScanCallback scan_float = std::bind(sscanf, std::placeholders::_1, "%f", std::placeholders::_2); - -PrintCallback print_int = std::bind(printf, "%d", std::placeholders::_1); -ScanCallback scan_int = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2); - -PrintCallback print_bool = std::bind(printf, "%d", std::placeholders::_1); -ScanCallback scan_bool = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2); - -PrintCallback print_uint16 = std::bind(printf, "%d", std::placeholders::_1); -ScanCallback scan_uint16 = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2); - class Endpoint { -// Oskar: public before private: https://google.github.io/styleguide/cppguide.html#Declaration_Order -private: - // Oskar: variable naming: underscores at end not beginning of Class Data Members, - // https://google.github.io/styleguide/cppguide.html#Variable_Names - const TypeInfo_t _type_info; - const PrintCallback _print_callback; - ScanCallback _scan_callback; - const void* const _ctx; - public: - const char* const _name; + const char* const name_; Endpoint(const char* name, TypeInfo_t type_info, PrintCallback print_callback, const void *ctx) : - _type_info(type_info), - _print_callback(print_callback), - _scan_callback(nullptr), - _ctx(ctx), - _name(name) + name_(name), + type_info_(type_info), + print_callback_(print_callback), + scan_callback_(nullptr), + ctx_(ctx) { } Endpoint(const char* name, TypeInfo_t type_info, PrintCallback print_callback, ScanCallback scan_callback, void *ctx) : - _type_info(type_info), - _print_callback(print_callback), - _scan_callback(scan_callback), - _ctx(ctx), - _name(name) + name_(name), + type_info_(type_info), + print_callback_(print_callback), + scan_callback_(scan_callback), + ctx_(ctx) { } - //Oskar: very nice that you omit scan feature when const. - // It would be cool if you can relay this writable info in the JSON, - // so that something like the GUI can draw a grayed out box. Endpoint(const char* name, const float& ctx) : - Endpoint(name, AS_FLOAT, print_float, &ctx) {} + Endpoint(name, AS_FLOAT, Protocol_print_simple_type, &ctx) {} Endpoint(const char* name, float& ctx) : - Endpoint(name, AS_FLOAT, print_float, scan_float, &ctx) {} + Endpoint(name, AS_FLOAT, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {} Endpoint(const char* name, const int& ctx) : - Endpoint(name, AS_INT, print_int, &ctx) {} + Endpoint(name, AS_INT, Protocol_print_simple_type, &ctx) {} Endpoint(const char* name, int& ctx) : - Endpoint(name, AS_INT, print_int, scan_int, &ctx) {} + Endpoint(name, AS_INT, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {} Endpoint(const char* name, const bool& ctx) : - Endpoint(name, AS_BOOL, print_bool, &ctx) {} + Endpoint(name, AS_BOOL, Protocol_print_simple_type, &ctx) {} Endpoint(const char* name, bool& ctx) : - Endpoint(name, AS_BOOL, print_bool, scan_bool, &ctx) {} + Endpoint(name, AS_BOOL, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {} Endpoint(const char* name, const uint16_t& ctx) : - Endpoint(name, AS_UINT16, print_uint16, &ctx) {} + Endpoint(name, AS_UINT16, Protocol_print_simple_type, &ctx) {} Endpoint(const char* name, uint16_t& ctx) : - Endpoint(name, AS_UINT16, print_uint16, scan_uint16, &ctx) {} + Endpoint(name, AS_UINT16, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {} - void print_json(size_t id, bool& need_comma) { - if (_type_info == END_TREE) { - printf("]}"); - need_comma = true; - return; - } else if (_type_info < END_TREE) { - assert(_name); - assert(_type_names[_type_info]); - if (need_comma) - printf(","); - printf("{\"name\":\"%s\",\"id\":%u,\"type\":\"%s\"", _name, id, _type_names[_type_info]); - if (_type_info == BEGIN_TREE) { - printf(",\"content\":["); - need_comma = false; - } else { - printf("}"); - need_comma = true; - } - } - } + void print_json(size_t id, bool& need_comma); void print_value(void) { - if (_print_callback) - _print_callback(_ctx); + if (print_callback_) + print_callback_(type_info_, ctx_); } void scan_value(const char* buffer) { - if (_scan_callback) - _scan_callback(buffer, const_cast(_ctx)); + if (scan_callback_) + scan_callback_(type_info_, buffer, const_cast(ctx_)); } + +private: + const TypeInfo_t type_info_; + const PrintCallback print_callback_; + ScanCallback scan_callback_; + const void* const ctx_; };