diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 02a1edc6..8fab2783 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -130,10 +130,12 @@ bool Axis::wait_for_current_meas() { // step/direction interface void Axis::step_cb() { - const bool dir_pin = dir_gpio_.read(); - const int32_t dir = (-1 + 2 * dir_pin) * step_dir_active_; - controller_.input_pos_ += dir * config_.turns_per_step; - controller_.input_pos_updated(); + if (step_dir_active_) { + const bool dir_pin = dir_gpio_.read(); + const float dir = dir_pin ? 1.0f : -1.0f; + controller_.input_pos_ += dir * config_.turns_per_step; + controller_.input_pos_updated(); + } } void Axis::decode_step_dir_pins() { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 09ab273f..f9e4eb7d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -10,6 +10,7 @@ class Axis; #include "endstop.hpp" #include "low_level.h" #include "utils.hpp" +#include "communication/interface_uart.h" // TODO: remove once uart_poll() is gone #include @@ -169,6 +170,10 @@ public: // TODO: change arming logic to arm after waiting bool main_continue = update_handler(); + if (axis_num_ == 0) { + uart_poll(); // TODO: move to board-level control loop once it exists + } + // Check we meet deadlines after queueing ++loop_counter_; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 03228893..2a5c9f5c 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -8,6 +8,7 @@ #include #include #include +#include extern "C" { #endif diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 31589f0c..ae23a614 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -31,6 +31,21 @@ static Introspectable root_obj = ODriveTypeInfo::make_introspectable(odrv); /* Private function prototypes -----------------------------------------------*/ + +void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_help(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_info_dump(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum); + /* Function implementations --------------------------------------------------*/ // @brief Sends a line on the specified output. @@ -85,7 +100,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& bool use_checksum = (checksum_start < len); if (use_checksum) { unsigned int received_checksum; - int numscan = sscanf((const char *)cmd + checksum_start, "%u", &received_checksum); + int numscan = sscanf(&cmd[checksum_start], "%u", &received_checksum); if ((numscan < 1) || (received_checksum != checksum)) return; len = checksum_start - 1; // prune checksum and asterisk @@ -94,196 +109,283 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& // check incoming packet type - if (cmd[0] == 'p') { // position control - unsigned motor_number; - float pos_setpoint, vel_feed_forward, torque_feed_forward; - int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &torque_feed_forward); - if (numscan < 2) { - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - } else { - Axis& axis = axes[motor_number]; - axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; - axis.controller_.input_pos_ = pos_setpoint; - if (numscan >= 3) - axis.controller_.input_vel_ = vel_feed_forward; - if (numscan >= 4) - axis.controller_.input_torque_ = torque_feed_forward; - axis.controller_.input_pos_updated(); - axis.watchdog_feed(); - } - - } else if (cmd[0] == 'q') { // position control with limits - unsigned motor_number; - float pos_setpoint, vel_limit, torque_lim; - int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &torque_lim); - if (numscan < 2) { - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - } else { - Axis& axis = axes[motor_number]; - axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; - axis.controller_.input_pos_ = pos_setpoint; - if (numscan >= 3) - axis.controller_.config_.vel_limit = vel_limit; - if (numscan >= 4) - axis.motor_.config_.torque_lim = torque_lim; - axis.controller_.input_pos_updated(); - axis.watchdog_feed(); - } - - } else if (cmd[0] == 'v') { // velocity control - unsigned motor_number; - float vel_setpoint, torque_feed_forward; - int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, &torque_feed_forward); - if (numscan < 2) { - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - } else { - Axis& axis = axes[motor_number]; - axis.controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; - axis.controller_.input_vel_ = vel_setpoint; - if (numscan >= 3) - axis.controller_.input_torque_ = torque_feed_forward; - axis.watchdog_feed(); - } - - } else if (cmd[0] == 'c') { // torque control - unsigned motor_number; - float torque_setpoint; - int numscan = sscanf(cmd, "c %u %f", &motor_number, &torque_setpoint); - if (numscan < 2) { - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - } else { - Axis& axis = axes[motor_number]; - axis.controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL; - axis.controller_.input_torque_ = torque_setpoint; - axis.watchdog_feed(); - } - - } else if (cmd[0] == 't') { // trapezoidal trajectory - unsigned motor_number; - float goal_point; - int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point); - if (numscan < 2) { - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - } else { - Axis& axis = axes[motor_number]; - axis.controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; - axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; - axis.controller_.input_pos_ = goal_point; - axis.controller_.input_pos_updated(); - axis.watchdog_feed(); - } - - } else if (cmd[0] == 'f') { // feedback - unsigned motor_number; - int numscan = sscanf(cmd, "f %u", &motor_number); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - } else { - respond(response_channel, use_checksum, "%f %f", - (double)axes[motor_number].encoder_.pos_estimate_, - (double)axes[motor_number].encoder_.vel_estimate_); - } - - } else if (cmd[0] == 'h') { // Help - respond(response_channel, use_checksum, "Please see documentation for more details"); - respond(response_channel, use_checksum, ""); - respond(response_channel, use_checksum, "Available commands syntax reference:"); - respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); - respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); - respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); - respond(response_channel, use_checksum, "Torque: c axis T"); - respond(response_channel, use_checksum, ""); - respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); - respond(response_channel, use_checksum, "Read: r property"); - respond(response_channel, use_checksum, "Write: w property value"); - respond(response_channel, use_checksum, ""); - respond(response_channel, use_checksum, "Save config: ss"); - respond(response_channel, use_checksum, "Erase config: se"); - respond(response_channel, use_checksum, "Reboot: sr"); - - } else if (cmd[0] == 'i'){ // Dump device info - // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); - // respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision()); - // respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); - respond(response_channel, use_checksum, "Hardware version: %d.%d-%dV", odrv.hw_version_major_, odrv.hw_version_minor_, odrv.hw_version_variant_); - respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", odrv.fw_version_major_, odrv.fw_version_minor_, odrv.fw_version_revision_); - respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); - - } else if (cmd[0] == 's'){ // System - if(cmd[1] == 's') { // Save config - odrv.save_configuration(); - } else if (cmd[1] == 'e'){ // Erase config - odrv.erase_configuration(); - } else if (cmd[1] == 'r'){ // Reboot - odrv.reboot(); - } - - } else if (cmd[0] == 'r') { // read property - char name[MAX_LINE_LENGTH]; - int numscan = sscanf(cmd, "r %255s", name); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else { - Introspectable property = root_obj.get_child(name, sizeof(name)); - const StringConvertibleTypeInfo* type_info = dynamic_cast(property.get_type_info()); - if (!type_info) { - respond(response_channel, use_checksum, "invalid property"); - } else { - char response[10]; - bool success = type_info->get_string(property, response, sizeof(response)); - if (!success) - respond(response_channel, use_checksum, "not implemented"); - else - respond(response_channel, use_checksum, response); - } - } - - } else if (cmd[0] == 'w') { // write property - char name[MAX_LINE_LENGTH]; - char value[MAX_LINE_LENGTH]; - int numscan = sscanf(cmd, "w %255s %255s", name, value); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else { - Introspectable property = root_obj.get_child(name, sizeof(name)); - const StringConvertibleTypeInfo* type_info = dynamic_cast(property.get_type_info()); - if (!type_info) { - respond(response_channel, use_checksum, "invalid property"); - } else { - bool success = type_info->set_string(property, value, sizeof(value)); - if (!success) - respond(response_channel, use_checksum, "not implemented"); - } - } - - } else if (cmd[0] == 'u') { // Update axis watchdog. - unsigned motor_number; - int numscan = sscanf(cmd, "u %u", &motor_number); - if(numscan < 1){ - respond(response_channel, use_checksum, "invalid command format"); - } else if (motor_number >= AXIS_COUNT) { - respond(response_channel, use_checksum, "invalid motor %u", motor_number); - }else { - axes[motor_number].watchdog_feed(); - } - - } else if (cmd[0] != 0) { - respond(response_channel, use_checksum, "unknown command"); + switch(cmd[0]) { + case 'p': cmd_set_position(cmd, response_channel, use_checksum); break; // position control + case 'q': cmd_set_position_wl(cmd, response_channel, use_checksum); break; // position control with limits + case 'v': cmd_set_velocity(cmd, response_channel, use_checksum); break; // velocity control + case 'c': cmd_set_torque(cmd, response_channel, use_checksum); break; // current control + case 't': cmd_set_trapezoid_trajectory(cmd, response_channel, use_checksum); break; // trapezoidal trajectory + case 'f': cmd_get_feedback(cmd, response_channel, use_checksum); break; // feedback + case 'h': cmd_help(cmd, response_channel, use_checksum); break; // Help + case 'i': cmd_info_dump(cmd, response_channel, use_checksum); break; // Dump device info + case 's': cmd_system_ctrl(cmd, response_channel, use_checksum); break; // System + case 'r': cmd_read_property(cmd, response_channel, use_checksum); break; // read property + case 'w': cmd_write_property(cmd, response_channel, use_checksum); break; // write property + case 'u': cmd_update_axis_wdg(cmd, response_channel, use_checksum); break; // Update axis watchdog. + default : cmd_unknown(nullptr, response_channel, use_checksum); break; } } +// @brief Executes the set position command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + float pos_setpoint, vel_feed_forward, torque_feed_forward; + + int numscan = sscanf(pStr, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &torque_feed_forward); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; + axis.controller_.input_pos_ = pos_setpoint; + if (numscan >= 3) + axis.controller_.input_vel_ = vel_feed_forward; + if (numscan >= 4) + axis.controller_.input_torque_ = torque_feed_forward; + axis.controller_.input_pos_updated(); + axis.watchdog_feed(); + } +} + +// @brief Executes the set position with current and velocity limit command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + float pos_setpoint, vel_limit, torque_lim; + + int numscan = sscanf(pStr, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &torque_lim); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; + axis.controller_.input_pos_ = pos_setpoint; + if (numscan >= 3) + axis.controller_.config_.vel_limit = vel_limit; + if (numscan >= 4) + axis.motor_.config_.torque_lim = torque_lim; + axis.controller_.input_pos_updated(); + axis.watchdog_feed(); + } +} + +// @brief Executes the set velocity command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + float vel_setpoint, torque_feed_forward; + int numscan = sscanf(pStr, "v %u %f %f", &motor_number, &vel_setpoint, &torque_feed_forward); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; + axis.controller_.input_vel_ = vel_setpoint; + if (numscan >= 3) + axis.controller_.input_torque_ = torque_feed_forward; + axis.watchdog_feed(); + } +} + +// @brief Executes the set torque control command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + float torque_setpoint; + + if (sscanf(pStr, "c %u %f", &motor_number, &torque_setpoint) < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL; + axis.controller_.input_torque_ = torque_setpoint; + axis.watchdog_feed(); + } +} + +// @brief Executes the set trapezoid trajectory command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + float goal_point; + + if (sscanf(pStr, "t %u %f", &motor_number, &goal_point) < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + axis.controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; + axis.controller_.input_pos_ = goal_point; + axis.controller_.input_pos_updated(); + axis.watchdog_feed(); + } +} + +// @brief Executes the get position and velocity feedback command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + + if (sscanf(pStr, "f %u", &motor_number) < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + respond(response_channel, use_checksum, "%f %f", + (double)axis.encoder_.pos_estimate_, + (double)axis.encoder_.vel_estimate_); + } +} + +// @brief Shows help text +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_help(char * pStr, StreamSink& response_channel, bool use_checksum) { + (void)pStr; + respond(response_channel, use_checksum, "Please see documentation for more details"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Available commands syntax reference:"); + respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); + respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); + respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); + respond(response_channel, use_checksum, "Torque: c axis T"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); + respond(response_channel, use_checksum, "Read: r property"); + respond(response_channel, use_checksum, "Write: w property value"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Save config: ss"); + respond(response_channel, use_checksum, "Erase config: se"); + respond(response_channel, use_checksum, "Reboot: sr"); +} + +// @brief Gets the hardware, firmware and serial details +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_info_dump(char * pStr, StreamSink& response_channel, bool use_checksum) { + // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); + // respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision()); + // respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); + respond(response_channel, use_checksum, "Hardware version: %d.%d-%dV", odrv.hw_version_major_, odrv.hw_version_minor_, odrv.hw_version_variant_); + respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", odrv.fw_version_major_, odrv.fw_version_minor_, odrv.fw_version_revision_); + respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); +} + +// @brief Executes the system control command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum) { + switch (pStr[1]) + { + case 's': odrv.save_configuration(); break; // Save config + case 'e': odrv.erase_configuration(); break; // Erase config + case 'r': odrv.reboot(); break; // Reboot + default: /* default */ break; + } +} + +// @brief Executes the read parameter command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum) { + char name[MAX_LINE_LENGTH]; + + if (sscanf(pStr, "r %255s", name) < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Introspectable property = root_obj.get_child(name, sizeof(name)); + const StringConvertibleTypeInfo* type_info = dynamic_cast(property.get_type_info()); + if (!type_info) { + respond(response_channel, use_checksum, "invalid property"); + } else { + char response[10]; + bool success = type_info->get_string(property, response, sizeof(response)); + respond(response_channel, use_checksum, success ? response : "not implemented"); + } + } +} + +// @brief Executes the set write position command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum) { + char name[MAX_LINE_LENGTH]; + char value[MAX_LINE_LENGTH]; + + if (sscanf(pStr, "w %255s %255s", name, value) < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Introspectable property = root_obj.get_child(name, sizeof(name)); + const StringConvertibleTypeInfo* type_info = dynamic_cast(property.get_type_info()); + if (!type_info) { + respond(response_channel, use_checksum, "invalid property"); + } else { + bool success = type_info->set_string(property, value, sizeof(value)); + if (!success) { + respond(response_channel, use_checksum, "not implemented"); + } + } + } +} + +// @brief Executes the motor watchdog update command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum) { + unsigned motor_number; + + if (sscanf(pStr, "u %u", &motor_number) < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + axes[motor_number].watchdog_feed(); + } +} + +// @brief Sends the unknown command response +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum) { + (void)pStr; + respond(response_channel, use_checksum, "unknown command"); +} + +// @brief Parses the received ASCII char stream +// @param buffer buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) { static uint8_t parse_buffer[MAX_LINE_LENGTH]; static bool read_active = true; diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index e899325d..ee10f29a 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -64,9 +64,7 @@ static void uart_server_thread(void * ctx) { (void) ctx; for (;;) { - osDelay(1); - - // Check for UART errors and restart recieve DMA transfer if required + // Check for UART errors and restart receive DMA transfer if required if (huart_->RxState != HAL_UART_STATE_BUSY_RX) { HAL_UART_AbortReceive(huart_); HAL_UART_Receive_DMA(huart_, dma_rx_buffer, sizeof(dma_rx_buffer)); @@ -94,12 +92,18 @@ static void uart_server_thread(void * ctx) { new_rcv_idx - dma_last_rcv_idx, uart_stream_output); dma_last_rcv_idx = new_rcv_idx; } - }; + + // The thread is woken up by the control loop at 8kHz. This should be + // enough for most applications. + // At 1Mbaud/s that corresponds to at most 12.5 bytes which can arrive + // during the sleep period. + osThreadSuspend(nullptr); + } } // TODO: allow multiple UART server instances void start_uart_server() { - // DMA is set up to recieve in a circular buffer forever. + // DMA is set up to receive in a circular buffer forever. // We dont use interrupts to fetch the data, instead we periodically read // data out of the circular buffer into a parse buffer, controlled by a state machine HAL_UART_Receive_DMA(huart_, dma_rx_buffer, sizeof(dma_rx_buffer)); @@ -110,6 +114,10 @@ void start_uart_server() { uart_thread = osThreadCreate(osThread(uart_server_thread_def), NULL); } +void uart_poll() { + osThreadResume(uart_thread); +} + void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { osSemaphoreRelease(sem_uart_dma); } diff --git a/Firmware/communication/interface_uart.h b/Firmware/communication/interface_uart.h index 362d417c..a7df55bd 100644 --- a/Firmware/communication/interface_uart.h +++ b/Firmware/communication/interface_uart.h @@ -13,7 +13,8 @@ extern "C" { extern osThreadId uart_thread; extern const uint32_t stack_size_uart_thread; -void start_uart_server(); +void start_uart_server(void); +void uart_poll(void); #ifdef __cplusplus } diff --git a/analysis/Simulation/MotorSim.py b/analysis/Simulation/MotorSim.py new file mode 100644 index 00000000..6d457db1 --- /dev/null +++ b/analysis/Simulation/MotorSim.py @@ -0,0 +1,225 @@ +# this file is for the simulation of a 3-phase synchronous motor + +import numpy as np +import scipy as sp +import scipy.signal as signal +import scipy.integrate +import matplotlib.pyplot as plt +import time + +def sign(num): + if num > 0: + return 1 + elif num < 0: + return -1 + else: + return 0 + +C = np.array([0, 1/5, 3/10, 4/5, 8/9, 1]) +A = np.array([ + [0, 0, 0, 0, 0], + [1/5, 0, 0, 0, 0], + [3/40, 9/40, 0, 0, 0], + [44/45, -56/15, 32/9, 0, 0], + [19372/6561, -25360/2187, 64448/6561, -212/729, 0], + [9017/3168, -355/33, 46732/5247, 49/176, -5103/18656] +]) +B = np.array([35/384, 0, 500/1113, 125/192, -2187/6784, 11/84]) + +# rk_step from scipy.integrate rk.py +def rk_step(fun, t, y, f, h, A, B, C, K): + """Perform a single Runge-Kutta step. + This function computes a prediction of an explicit Runge-Kutta method and + also estimates the error of a less accurate method. + Notation for Butcher tableau is as in [1]_. + Parameters + ---------- + fun : callable + Right-hand side of the system. + t : float + Current time. + y : ndarray, shape (n,) + Current state. + f : ndarray, shape (n,) + Current value of the derivative, i.e., ``fun(x, y)``. + h : float + Step to use. + A : ndarray, shape (n_stages, n_stages) + Coefficients for combining previous RK stages to compute the next + stage. For explicit methods the coefficients at and above the main + diagonal are zeros. + B : ndarray, shape (n_stages,) + Coefficients for combining RK stages for computing the final + prediction. + C : ndarray, shape (n_stages,) + Coefficients for incrementing time for consecutive RK stages. + The value for the first stage is always zero. + K : ndarray, shape (n_stages + 1, n) + Storage array for putting RK stages here. Stages are stored in rows. + The last row is a linear combination of the previous rows with + coefficients + Returns + ------- + y_new : ndarray, shape (n,) + Solution at t + h computed with a higher accuracy. + f_new : ndarray, shape (n,) + Derivative ``fun(t + h, y_new)``. + References + ---------- + .. [1] E. Hairer, S. P. Norsett G. Wanner, "Solving Ordinary Differential + Equations I: Nonstiff Problems", Sec. II.4. + """ + K[0] = f + for s, (a, c) in enumerate(zip(A[1:], C[1:]), start=1): + dy = np.dot(K[:s].T, a[:s]) * h + K[s] = fun(t + c * h, y + dy) + + y_new = y + h * np.dot(K[:-1].T, B) + f_new = fun(t + h, y_new) + + K[-1] = f_new + + return y_new, f_new + + +# example params for d5065 motor +# phase_R = 0.039 Ohms +# phase_L = 0.0000157 H +# pole_pairs = 7 +# KV = 270 +# J = 1e-4 +# b_coulomb = 0.001 +# b_viscous = 0.001 + +class motor_pmsm_mechanical: + def __init__(self, J, b_coulomb, b_viscous): + # J is moment of inertia + # b_coulomb is coulomb friction coefficient + # b_viscous is viscous friction coefficient + self.J = J + self.b_c = b_coulomb + self.b_v = b_viscous + + def diff_eqs(self, t, y, torque): + theta = y[0] + theta_dot = y[1] + + theta_ddot = (1/self.J) * (torque - self.b_v * theta_dot - self.b_c * sign(theta_dot)) + + return np.array([theta_dot, theta_ddot]) + +def inverter(vbus, timings, current): + # this function should take the relevant inputs and output voltages in dq reference frame. + pass + +class motor: + def __init__(self, J, b_coulomb, b_viscous, R, L_q, L_d, KV, pole_pairs, dT): + self.dT = dT + self.b_coulomb = b_coulomb + self.b_viscous = b_viscous + self.KV = KV + self.pole_pairs = pole_pairs + kt = 8.27/KV + self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad) + self.R = R + self.L_q = L_q + self.L_d = L_d + self.J = J + + # state variables for motor + self.theta = 0 # mechanical! + self.theta_dot = 0 # mechanical! + self.I_d = 0 + self.I_q = 0 + + # K matrix. For integrator? + # np.empty((self.n_stages + 1, self.n_stages), dtype=self.y.dtype) + self.K = np.empty((7, 4)) + + def simulate(self, t, u, x0): + # t is timesteps [t0, t1, ...] + # u is [T_load, V_d, V_q] + # x0 is initial states, [theta, theta_dot, I_d, I_q] + (self.theta, self.theta_dot, self.I_d, self.I_q) = x0 + time = [] + pos = [] + vel = [] + I_d = [] + I_q = [] + for i in range(len(t)): + self.single_step_rk(u[2],u[1],u[0]) + time.append(i*self.dT) + pos.append(self.theta) + vel.append(self.theta_dot) + I_d.append(self.I_d) + I_q.append(self.I_q) + + return [time,pos,vel,I_d,I_q] + + def inputs(self, V_q, V_d, T_load): + self.V_q = V_q + self.V_d = V_d + self.T_load = T_load + + def diff_eqs(self, t, y): + # inputs are self.V_q, self.V_d, self.T_load + # state is y, y = [theta, theta_dot, I_d, I_q] + # set_inputs must be called before this if the inputs have changed. + theta = y[0] + theta_dot = y[1] + I_d = y[2] + I_q = y[3] + + torque = 3*self.pole_pairs/2 * (self.lambda_m * I_q + (self.L_d - self.L_q)*I_d*I_q) - self.T_load + + if theta_dot == 0 and -1*self.b_coulomb < torque < self.b_coulomb: + torque = 0 + + # theta_dot = theta_dot, no ode here + theta_ddot = (1/self.J) * (torque - self.b_viscous * theta_dot - self.b_coulomb * sign(theta_dot)) + I_d_dot = self.V_d / self.L_d - self.R / self.L_d * I_d + theta_dot*self.pole_pairs * self.L_q / self.L_d * I_q + I_q_dot = self.V_q / self.L_q - self.R / self.L_q * I_q - theta_dot*self.pole_pairs * self.L_d / self.L_q * I_d - theta_dot*self.pole_pairs * self.lambda_m / self.L_q + + return np.array([theta_dot, theta_ddot, I_d_dot, I_q_dot]) + + def single_step_rk(self, V_q, V_d, T_load): + # given inputs + self.inputs(V_q, V_d, T_load) + x = (d5065.theta, d5065.theta_dot, d5065.I_d, d5065.I_q) + ((d5065.theta, d5065.theta_dot, d5065.I_d, d5065.I_q), _) = rk_step(d5065.diff_eqs, 0, x, d5065.diff_eqs(0, x), d5065.dT, A, B, C, d5065.K) + +if __name__ == "__main__": + d5065 = motor(J = 1e-4, b_coulomb = 0, b_viscous = 0.01, R = 0.039, L_q = 1.57e-5, L_d = 1.57e-5, KV = 270, pole_pairs = 7, dT = 1/48000) + x0 = [0,0,0,0] # initial state of theta, theta_dot, I_d, I_q + u = [0,0,1] # input for simulation as [T_load, V_d, V_q] + t = [i*1/48000 for i in range(12000)] # half second of runtime at Fs=48kHz + + data = d5065.simulate(t=t, u=u, x0=x0) + dT = 1/48000 + states = [] + pos = [] + vel = [] + I_d = [] + I_q = [] + pos = data[1] + vel = data[2] + I_d = data[3] + I_q = data[4] + + fig, axs = plt.subplots(4) + + axs[0].plot(t, pos) + axs[0].set_title('pos') + axs[0].set_ylabel('Theta (eRad)') + axs[1].plot(t, vel) + axs[1].set_title('vel') + axs[1].set_ylabel('Omega (eRad/s)') + axs[2].plot(t,I_d) + axs[2].set_title('I_d') + axs[2].set_ylabel('Current (A)') + axs[3].plot(t,I_q) + axs[3].set_title('I_q') + axs[3].set_ylabel('Current (A)') + axs[3].set_xlabel('time (s)') + + plt.show() \ No newline at end of file diff --git a/analysis/Simulation/TranslationalMass.py b/analysis/Simulation/TranslationalMass.py index 3cb1a4aa..db7fbd64 100644 --- a/analysis/Simulation/TranslationalMass.py +++ b/analysis/Simulation/TranslationalMass.py @@ -1,6 +1,8 @@ import os import matplotlib.pyplot as plt from control.matlab import * +import numpy as np + # Input: Current (A) # Output: Torque (Nm) @@ -26,10 +28,35 @@ def mass(m, b, k): def pulley(r): return tf(r, 1) -sys = series(motor(2.5), pulley(0.015), mass(0.10, 0, 0)) +# Make s a transfer function s/1 +s = tf('s') +print(s) + +# build a new transfer function using our variable s as a handy placeholder +sys = 1 / (s*s + s + 1) +print(sys) + +# Hit the system with a step command +yout, T = step(sys) +plt.plot(T, yout) + +# convert our continuous time model to discrete time via Tustin at 0.01s timestep +sysd = c2d(tf(sys), 0.01, method='tustin') +print(sysd) + +# Hit the discrete system with a step command, and sample it at 0.01 timestep from 0 to 14 seconds +yout, T = step(sysd, np.arange(0, 14, 0.01)) +plt.plot(T, yout) +plt.legend(['Continuous', 'Discrete']) + +# Build a system based on the series connection of the motor, pulley, and mass "blocks" +sys = series(motor(2.5), pulley(0.015), mass(0.10, .1, .1)) +print(tf(sys)) + +# Step our series system, returning y (outputs) and x (states) yout, T, xout = step(sys, return_x=True) -print(yout) -# plt.plot(T, yout) +plt.figure() +plt.plot(T, yout) plt.plot(T, xout) -plt.legend(['Displacement', 'Velocity']) +plt.legend(['Displacement', r'$x$', r'$\dot{x}$']) plt.show() \ No newline at end of file diff --git a/docs/anticogging.md b/docs/anticogging.md new file mode 100644 index 00000000..91b494b5 --- /dev/null +++ b/docs/anticogging.md @@ -0,0 +1,53 @@ +# Anti-cogging + +ODrive supports an anti-cogging algorithm that attempts to compensate for the rather high cogging torques seen in hobby motors. + +`..controller.config.anticogging` is a configuration structure that contains the following items: + +Name | Type | Use +-- | -- | -- +index | uint32 | The current position being used for calibration +pre_calibrated | bool | If true and using index or absolute encoder, load anticogging map from NVM at startup +calib_anticogging | bool | True when calibration is ongoing +calib_pos_threshold | float32 | (pos_estimate - index) must be < this value to calibrate. Larger values speed up calibration but hurt accuracy +calib_vel_threshold | float32 | (vel_estimate) must be < this value to calibrate. Larger values speed up calibration but hurt accuracy. +cogging_ratio | float32 | Deprecated +anticogging_enabled | bool | Enable or disable anticogging. A valid anticogging map can be ignored by setting this to `false` + +## Calibration + +To calibrate anticogging, first make sure you can adequately control the motor in . It should respond to position commands. + +Start by putting the axis in `AXIS_STATE_CLOSED_LOOP` with `CONTROL_MODE_POSITION_CONTROL` and `INPUT_MODE_PASSTHROUGH`. Make sure you have good control of the motor in this state (it responds to position commands). Now, tune the motor to be very stiff - high `pos_gain` and relatively high `vel_integrator_gain`. This will help in calibration. + +Run `controller.start_anticogging_calibration()`. The motor will start turning slowly, calibrating each point. If you like, you can start a liveplotter session before running this command so that you can watch the position move. + +Once it's complete (it should take about 1 minute), the motor will return to 0 and the value `controller.anticogging_valid` should report True. If `controller.config.anticogging.anticogging_enabled` == True, anticogging will now be running on this axis. + +## Saving to NVM + +As of v0.5.1, the anticogging map is saved to NVM after calibrating and calling `odrv0.save_configuration()` + +The anticogging map can be reloaded automatically at startup by setting `controller.config.anticogging.pre_calibrated = True` and saving the configuration. However, this map is only valid and will only be loaded for absolute encoders, or encoders with index pins after the index search. + +## Example + +``` Py +odrv0.axis0.encoder.config.use_index = True +odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE +odrv0.axis0.encoder.config.pre_calibrated = True +odrv0.axis0.motor.config.pre_calibrated = True + +odrv0.axis0.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL +odrv0.axis0.controller.config.input_mode = INPUT_MODE_PASSTHROUGH +odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + +odrv0.axis0.controller.start_anticogging_calibration() + +# Wait until controller.config.anticogging.calib_anticogging == False + +odrv0.axis0.controller.config.anticogging.pre_calibrated = True + +odrv0.save_configuration() +odrv0.reboot() +``` diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index f185040c..e9ddc706 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -176,7 +176,7 @@ class TestRegenProtection(TestClosedLoopControlBase): def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): - nominal_rps = 15.0 + nominal_rps = 10.0 nominal_vel = nominal_rps max_current = 30.0