From 4c6fe56a5d99aa4442aabda6b1acca7f52051af5 Mon Sep 17 00:00:00 2001 From: sgilbert182 <45004203+sgilbert182@users.noreply.github.com> Date: Thu, 12 Dec 2019 10:12:07 +0000 Subject: [PATCH 01/12] Split up menu function to individual functions, changed if else tree to switch statement. --- Firmware/communication/ascii_protocol.cpp | 493 ++++++++++++++-------- 1 file changed, 308 insertions(+), 185 deletions(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 1cb5f794..a87c7388 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -11,7 +11,7 @@ #include "../build/version.h" // autogenerated based on Git state #include "communication.h" #include "ascii_protocol.hpp" -#include +#include #include /* Private macros ------------------------------------------------------------*/ @@ -26,20 +26,36 @@ /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ + +void setPosition(char * pStr, StreamSink& response_channel, bool use_checksum); +void setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum); +void setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum); +void setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum); +void setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_checksum); +void getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum); +void help(char * pStr, StreamSink& response_channel, bool use_checksum); +void infoDump(char * pStr, StreamSink& response_channel, bool use_checksum); +void systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum); +void readProperty(char * pStr, StreamSink& response_channel, bool use_checksum); +void writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum); +void updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum); +void unknownCMD(char * pStr, StreamSink& response_channel, bool use_checksum); + /* Function implementations --------------------------------------------------*/ // @brief Sends a line on the specified output. template void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) { char response[64]; + size_t len = snprintf(response, sizeof(response), fmt, std::forward(args)...); output.process_bytes((uint8_t*)response, len, nullptr); // TODO: use process_all instead - if (include_checksum) { + if (include_checksum) + { uint8_t checksum = 0; for (size_t i = 0; i < len; ++i) checksum ^= response[i]; - len = snprintf(response, sizeof(response), "*%u", checksum); - output.process_bytes((uint8_t*)response, len, nullptr); + output.process_bytes((uint8_t*)response, snprintf(response, sizeof(response), "*%u", checksum), nullptr); } output.process_bytes((const uint8_t*)"\r\n", 2, nullptr); } @@ -59,7 +75,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& len = i; break; } - if (checksum_start > i) { + if (checksum_start > i) + { if (buffer[i] == '*') { checksum_start = i + 1; } else { @@ -72,200 +89,306 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& char cmd[MAX_LINE_LENGTH + 1]; if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH; memcpy(cmd, buffer, len); + cmd[len] = 0; // null-terminate // optional checksum validation bool use_checksum = (checksum_start < len); if (use_checksum) { unsigned int received_checksum; - sscanf((const char *)cmd + checksum_start, "%u", &received_checksum); - if (received_checksum != 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 + cmd[len] = 0; // null-terminate } - cmd[len] = 0; // null-terminate // check incoming packet type - if (cmd[0] == 'p') { // position control - unsigned motor_number; - float pos_setpoint, vel_feed_forward, current_feed_forward; - int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_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 { - if (numscan < 3) - vel_feed_forward = 0.0f; - if (numscan < 4) - current_feed_forward = 0.0f; - Axis* axis = axes[motor_number]; - axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); - axis->watchdog_feed(); - } - - } else if (cmd[0] == 'q') { // position control with limits - unsigned motor_number; - float pos_setpoint, vel_limit, current_lim; - int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_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_.pos_setpoint_ = pos_setpoint; - if (numscan >= 3) - axis->controller_.config_.vel_limit = vel_limit; - if (numscan >= 4) - axis->motor_.config_.current_lim = current_lim; - - axis->watchdog_feed(); - } - - } else if (cmd[0] == 'v') { // velocity control - unsigned motor_number; - float vel_setpoint, current_feed_forward; - int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_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 { - if (numscan < 3) - current_feed_forward = 0.0f; - Axis* axis = axes[motor_number]; - axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); - axis->watchdog_feed(); - } - - } else if (cmd[0] == 'c') { // current control - unsigned motor_number; - float current_setpoint; - int numscan = sscanf(cmd, "c %u %f", &motor_number, ¤t_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_.set_current_setpoint(current_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_.move_to_pos(goal_point); - 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, "Current: c axis I"); - 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", HW_VERSION_MAJOR, HW_VERSION_MINOR, HW_VERSION_VOLTAGE); - respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, 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 - save_configuration(); - } else if (cmd[1] == 'e'){ // Erase config - erase_configuration(); - } else if (cmd[1] == 'r'){ // Reboot - NVIC_SystemReset(); - } - - } else if (cmd[0] == 'r') { // read property - char name[MAX_LINE_LENGTH]; - int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else { - Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); - if (!endpoint) { - respond(response_channel, use_checksum, "invalid property"); - } else { - char response[10]; - bool success = endpoint->get_string(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 %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else { - Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); - if (!endpoint) { - respond(response_channel, use_checksum, "invalid property"); - } else { - bool success = endpoint->set_string(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': setPosition(cmd, response_channel, use_checksum); break; // position control + case 'q': setPositionWL(cmd, response_channel, use_checksum); break; // position control with limits + case 'v': setVelocity(cmd, response_channel, use_checksum); break; // velocity control + case 'c': setCurrent(cmd, response_channel, use_checksum); break; // current control + case 't': setTrapezoidTrajectory(cmd, response_channel, use_checksum); break; // trapezoidal trajectory + case 'f': getFeedback(cmd, response_channel, use_checksum); break; // feedback + case 'h': help(cmd, response_channel, use_checksum); break; // Help + case 'i': infoDump(cmd, response_channel, use_checksum); break; // Dump device info + case 's': systemCTRL(cmd, response_channel, use_checksum); break; // System + case 'r': readProperty(cmd, response_channel, use_checksum); break; // read property + case 'w': writeProperty(cmd, response_channel, use_checksum); break; // write property + case 'u': updateAxisWDG(cmd, response_channel, use_checksum); break; // Update axis watchdog. + default : unknownCMD(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 setPosition(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float pos_setpoint, vel_feed_forward, current_feed_forward; + + int numscan = sscanf(pStr, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_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::CTRL_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_current_ = current_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 setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float pos_setpoint, vel_limit, current_lim; + + int numscan = sscanf(pStr, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_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::CTRL_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_.current_lim = current_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 setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float vel_setpoint, current_feed_forward; + int numscan = sscanf(pStr, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_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::CTRL_MODE_VELOCITY_CONTROL; + axis->controller_.input_vel_ = vel_setpoint; + if (numscan >= 3) + axis->controller_.input_current_ = current_feed_forward; + axis->watchdog_feed(); + } +} + +// @brief Executes the set current 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 setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float current_setpoint; + + if (sscanf(pStr, "c %u %f", &motor_number, ¤t_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::CTRL_MODE_CURRENT_CONTROL; + axis->controller_.input_current_ = current_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 setTrapezoidTrajectory(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_.move_to_pos(goal_point); + 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 getFeedback(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 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, "Current: c axis I"); + 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 infoDump(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", HW_VERSION_MAJOR, HW_VERSION_MINOR, HW_VERSION_VOLTAGE); + respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, 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 systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + switch (pStr[1]) + { + case 's': save_configuration(); break; // Save config + case 'e': erase_configuration(); break; // Erase config + case 'r': NVIC_SystemReset(); 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 readProperty(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 { + Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + char response[10]; + respond(response_channel, use_checksum, (endpoint->get_string(response, sizeof(response))) ? 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 writeProperty(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 { + Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + if (!endpoint->set_string(value, sizeof(value))) { + 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 updateAxisWDG(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 unknownCMD(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; From b56f7c1ff2180a3c0e16b6bc41423b070c93b3dc Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 5 Aug 2020 18:49:48 +0200 Subject: [PATCH 02/12] harmonize coding style --- Firmware/communication/ascii_protocol.cpp | 103 +++++++++------------- 1 file changed, 44 insertions(+), 59 deletions(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index a87c7388..7e413ebe 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -27,19 +27,19 @@ /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ -void setPosition(char * pStr, StreamSink& response_channel, bool use_checksum); -void setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum); -void setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum); -void setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum); -void setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_checksum); -void getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum); -void help(char * pStr, StreamSink& response_channel, bool use_checksum); -void infoDump(char * pStr, StreamSink& response_channel, bool use_checksum); -void systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum); -void readProperty(char * pStr, StreamSink& response_channel, bool use_checksum); -void writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum); -void updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum); -void unknownCMD(char * pStr, StreamSink& response_channel, bool use_checksum); +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_current(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 --------------------------------------------------*/ @@ -50,12 +50,12 @@ void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs& size_t len = snprintf(response, sizeof(response), fmt, std::forward(args)...); output.process_bytes((uint8_t*)response, len, nullptr); // TODO: use process_all instead - if (include_checksum) - { + if (include_checksum) { uint8_t checksum = 0; for (size_t i = 0; i < len; ++i) checksum ^= response[i]; - output.process_bytes((uint8_t*)response, snprintf(response, sizeof(response), "*%u", checksum), nullptr); + len = snprintf(response, sizeof(response), "*%u", checksum); + output.process_bytes((uint8_t*)response, len, nullptr); } output.process_bytes((const uint8_t*)"\r\n", 2, nullptr); } @@ -75,8 +75,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& len = i; break; } - if (checksum_start > i) - { + if (checksum_start > i) { if (buffer[i] == '*') { checksum_start = i + 1; } else { @@ -104,21 +103,20 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& // check incoming packet type - switch(cmd[0]) - { - case 'p': setPosition(cmd, response_channel, use_checksum); break; // position control - case 'q': setPositionWL(cmd, response_channel, use_checksum); break; // position control with limits - case 'v': setVelocity(cmd, response_channel, use_checksum); break; // velocity control - case 'c': setCurrent(cmd, response_channel, use_checksum); break; // current control - case 't': setTrapezoidTrajectory(cmd, response_channel, use_checksum); break; // trapezoidal trajectory - case 'f': getFeedback(cmd, response_channel, use_checksum); break; // feedback - case 'h': help(cmd, response_channel, use_checksum); break; // Help - case 'i': infoDump(cmd, response_channel, use_checksum); break; // Dump device info - case 's': systemCTRL(cmd, response_channel, use_checksum); break; // System - case 'r': readProperty(cmd, response_channel, use_checksum); break; // read property - case 'w': writeProperty(cmd, response_channel, use_checksum); break; // write property - case 'u': updateAxisWDG(cmd, response_channel, use_checksum); break; // Update axis watchdog. - default : unknownCMD(nullptr, response_channel, use_checksum); break; + 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_current(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; } } @@ -126,8 +124,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& // @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 setPosition(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float pos_setpoint, vel_feed_forward, current_feed_forward; @@ -153,8 +150,7 @@ void setPosition(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float pos_setpoint, vel_limit, current_lim; @@ -180,8 +176,7 @@ void setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float vel_setpoint, current_feed_forward; int numscan = sscanf(pStr, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_feed_forward); @@ -203,8 +198,7 @@ void setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_set_current(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float current_setpoint; @@ -224,8 +218,7 @@ void setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float goal_point; @@ -245,8 +238,7 @@ void setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_ // @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 getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; if (sscanf(pStr, "f %u", &motor_number) < 1) { @@ -265,8 +257,7 @@ void getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 help(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +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, ""); @@ -289,8 +280,7 @@ void help(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 infoDump(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +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()); @@ -303,8 +293,7 @@ void infoDump(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum) { switch (pStr[1]) { case 's': save_configuration(); break; // Save config @@ -318,8 +307,7 @@ void systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 readProperty(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum) { char name[MAX_LINE_LENGTH]; if (sscanf(pStr, "r %255s", name) < 1) { @@ -339,8 +327,7 @@ void readProperty(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum) { char name[MAX_LINE_LENGTH]; char value[MAX_LINE_LENGTH]; @@ -362,8 +349,7 @@ void writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; if(sscanf(pStr, "u %u", &motor_number) < 1) { @@ -379,8 +365,7 @@ void updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 unknownCMD(char * pStr, StreamSink& response_channel, bool use_checksum) -{ +void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum) { (void)pStr; respond(response_channel, use_checksum, "unknown command"); } From 677133d85b6e9b5b99c28debf0219087e563a42e Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 5 Aug 2020 11:54:39 -0700 Subject: [PATCH 03/12] fix incorrect step_cb --- Firmware/MotorControl/axis.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 04a802c6..f5771cd2 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -118,10 +118,12 @@ bool Axis::wait_for_current_meas() { // step/direction interface void Axis::step_cb() { - const bool dir_pin = dir_port_->IDR & dir_pin_; - 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_port_->IDR & dir_pin_); + const float dir = dir_pin ? 1.0f : -1.0f; + controller_.input_pos_ += dir * config_.turns_per_step; + controller_.input_pos_updated(); + } }; void Axis::load_default_step_dir_pin_config( From 6fa76e1de802f82b946c9c3cd74deba9070144e8 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 7 Aug 2020 15:55:54 +0200 Subject: [PATCH 04/12] increase UART polling frequency from 1kHz to 8kHz --- Firmware/MotorControl/axis.hpp | 4 ++++ Firmware/MotorControl/odrive_main.h | 1 + Firmware/communication/interface_uart.cpp | 18 +++++++++++++----- Firmware/communication/interface_uart.h | 1 + 4 files changed, 19 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 446958d6..3271e432 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -163,6 +163,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 392b9c7e..d0dcface 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -12,6 +12,7 @@ #include #include #include +#include extern "C" { #endif diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index aa195574..08cd83f4 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -62,9 +62,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 (huart4.RxState != HAL_UART_STATE_BUSY_RX) { HAL_UART_AbortReceive(&huart4); HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); @@ -92,11 +90,17 @@ static void uart_server_thread(void * ctx) { new_rcv_idx - dma_last_rcv_idx, uart4_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); + } } 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(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); @@ -107,6 +111,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 65033a6f..18f1a2dc 100644 --- a/Firmware/communication/interface_uart.h +++ b/Firmware/communication/interface_uart.h @@ -14,6 +14,7 @@ extern osThreadId uart_thread; extern const uint32_t stack_size_uart_thread; void start_uart_server(void); +void uart_poll(void); #ifdef __cplusplus } From 1e39702e4ba3dc6c49c6ba92b71f748ae0e39c84 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Mon, 10 Aug 2020 18:11:55 -0400 Subject: [PATCH 05/12] Change back to 10rps for the regen protection HWIL test --- tools/odrive/tests/closed_loop_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index 263c8181..367813ea 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 From 9f1be401f61a28f887d139bc77d295649e081a45 Mon Sep 17 00:00:00 2001 From: Unknown Date: Thu, 13 Aug 2020 23:35:20 -0400 Subject: [PATCH 06/12] Add some example code for the python control module --- analysis/Simulation/TranslationalMass.py | 35 +++++++++++++++++++++--- 1 file changed, 31 insertions(+), 4 deletions(-) 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 From 7e3f6b6b7ebcf190d6df648b3163ab60489dcf1a Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 15 Aug 2020 18:18:59 -0400 Subject: [PATCH 07/12] Add anticogging.md --- docs/anticogging.md | 52 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 docs/anticogging.md diff --git a/docs/anticogging.md b/docs/anticogging.md new file mode 100644 index 00000000..2213c06c --- /dev/null +++ b/docs/anticogging.md @@ -0,0 +1,52 @@ +# 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.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() +``` \ No newline at end of file From 49c7942dcc7b4cd6d67e458095fac2439461305e Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 15 Aug 2020 18:39:16 -0400 Subject: [PATCH 08/12] Update anticogging.md --- docs/anticogging.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/anticogging.md b/docs/anticogging.md index 2213c06c..91b494b5 100644 --- a/docs/anticogging.md +++ b/docs/anticogging.md @@ -40,6 +40,7 @@ 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() @@ -49,4 +50,4 @@ odrv0.axis0.controller.config.anticogging.pre_calibrated = True odrv0.save_configuration() odrv0.reboot() -``` \ No newline at end of file +``` From 136cde76af582a79a75c7d9c4bcfed15b39977ff Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Sun, 16 Aug 2020 18:17:30 -0400 Subject: [PATCH 09/12] Started a file for PMSM and driver system simulation --- analysis/Simulation/MotorSim.py | 110 ++++++++++++++++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 analysis/Simulation/MotorSim.py diff --git a/analysis/Simulation/MotorSim.py b/analysis/Simulation/MotorSim.py new file mode 100644 index 00000000..adaa0775 --- /dev/null +++ b/analysis/Simulation/MotorSim.py @@ -0,0 +1,110 @@ +# this file is for the simulation of a 3-phase synchronous motor + +import numpy as np +import scipy as sp + +def sign(num): + if num > 0: + return 1 + elif num < 0: + return -1 + else: + return 0 + +# example params for d5065 motor +# phase_R = 0.039 Ohms +# phase_L = 0.0000157 H +# pole_pairs = 7 +# KV = 270 + +class motor_pmsm_electrical: + # class for simulating PMSM electrical dynamics in dq reference frame + # R, L input are phase-neutral, not phase-phase + def __init__(self, R, L_q, L_d, KV, pole_pairs): + # KV is published KV + kt = 8.27/KV + self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad) + self.L_q = L_q + self.L_d = L_d + self.R = R + self.pole_pairs = pole_pairs + + self.V_q = 0 + self.V_d = 0 + self.w_e = 0 + + def diff_eqs(self, t, y, V_d, V_q, w): + # this is for solving with solve_ivp or similar + # t is time, y is vector of state: [I_d, I_q], V_d and V_q are input voltages in dq ref frame, w is electrical freq + I_d = y[0] + I_q = y[1] + + # set these equal to the inputs for plotting + self.V_d = V_d + self.V_q = V_q + self.w_e = w + + I_d_dot = V_d / self.L_d - self.R / self.L_d * I_d + w * self.L_q / self.L_d * I_q + I_q_dot = V_q / self.L_q - self.R / self.L_q * I_q - w * self.L_d / self.L_q * I_d - w * self.lambda_m / self.L_q + + return np.array([I_d_dot, I_q_dot]) + +# example params for D5065 motor +# 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]) + +class motor_pmsm_combined: + def __init__(self, J, b_coulomb, b_viscous, R, L_q, L_d, KV, pole_pairs): + # 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 + + kt = 8.27/KV + self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad) + self.L_q = L_q + self.L_d = L_d + self.R = R + self.pole_pairs = pole_pairs + + def diff_eqs(self, t, y, V_d, V_q): + # inputs are V_d, V_q + # state is y, y = [theta, theta_dot, I_d, I_q] + + 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) + + # theta_dot = theta_dot, no ode here + theta_ddot = (1/self.J) * (torque - self.b_v * theta_dot - self.b_c * sign(theta_dot)) + I_d_dot = V_d / self.L_d - self.R / self.L_d * I_d + theta_dot * self.L_q / self.L_d * I_q + I_q_dot = V_q / self.L_q - self.R / self.L_q * I_q - theta_dot * self.L_d / self.L_q * I_d - theta_dot * self.lambda_m / self.L_q + + return np.array([theta_dot, theta_ddot, I_d_dot, I_q_dot]) + +def inverter(vbus, timings, current): + # this function should take the relevant inputs and output voltages in dq reference frame. + pass \ No newline at end of file From 91532855afd6bed61d53ca452de460baed0004fb Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Mon, 17 Aug 2020 20:28:50 -0400 Subject: [PATCH 10/12] Added `motor` class. Almost the same results as using solve_ivp, but this system is suitable for arbitrary discrete input, rather than an initial value problem. --- analysis/Simulation/MotorSim.py | 144 +++++++++++++++++++++++++++++++- 1 file changed, 141 insertions(+), 3 deletions(-) diff --git a/analysis/Simulation/MotorSim.py b/analysis/Simulation/MotorSim.py index adaa0775..721b9812 100644 --- a/analysis/Simulation/MotorSim.py +++ b/analysis/Simulation/MotorSim.py @@ -2,6 +2,9 @@ import numpy as np import scipy as sp +import scipy.signal as signal +import scipy.integrate +import matplotlib.pyplot as plt def sign(num): if num > 0: @@ -100,11 +103,146 @@ class motor_pmsm_combined: # theta_dot = theta_dot, no ode here theta_ddot = (1/self.J) * (torque - self.b_v * theta_dot - self.b_c * sign(theta_dot)) - I_d_dot = V_d / self.L_d - self.R / self.L_d * I_d + theta_dot * self.L_q / self.L_d * I_q - I_q_dot = V_q / self.L_q - self.R / self.L_q * I_q - theta_dot * self.L_d / self.L_q * I_d - theta_dot * self.lambda_m / self.L_q + I_d_dot = 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 = 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 inverter(vbus, timings, current): # this function should take the relevant inputs and output voltages in dq reference frame. - pass \ No newline at end of file + 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.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 + # set up SS matrices + # _m for mechanical, _e for electrical + self.A_m = np.array([[0,1],[0,-1*b_viscous/J]]) + self.B_m = np.array([[0],[1/J]]) + self.C_m = np.array([[1,0],[0,1]]) + self.D_m = np.array([[0],[0]]) + + self.A_e = np.array([[-1*R/L_d, 0],[0, -1*R/L_q]]) # the zero terms get replaced by the theta_dot terms in simulate + self.B_e = np.array([[1/L_d, 0],[0, 1/L_q]]) + self.C_e = np.array([[1,0],[0,1]]) + self.D_e = np.array([[0,0],[0,0]]) + + self.theta = 0 # mechanical! + self.theta_dot = 0 # mechanical! + self.I_d = 0 + self.I_q = 0 + + 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)): + out = self.singleStep(u[1],u[2],u[0]) + self.theta = out[0] + self.theta_dot = out[1] + self.I_d = out[2] + self.I_q = out[3] + 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 singleStep(self, V_d, V_q, T_load): + # create the discretized SS electrical and mechanical models, valid for this time instance + theta_e = self.theta * self.pole_pairs + theta_dot_e = self.theta_dot * self.pole_pairs + + V_q_effective = V_q - theta_dot_e * self.lambda_m + + # make new A electrical matrix with the weird coupled theta_dot terms + A_e = np.add(self.A_e, np.array([[0, theta_dot_e * self.L_q / self.L_d],[-1*theta_dot_e * self.L_d / self.L_q, 0]])) + + # SS_e is a discretized version of the electrical model, only valid for this time step (theta_dot will change) + SS_e = signal.cont2discrete((A_e, self.B_e, self.C_e, self.D_e), self.dT) + Ad_e = SS_e[0] # discretized A matrix for electrical system + Bd_e = SS_e[1] + Cd_e = SS_e[2] + Dd_e = SS_e[3] + + # do the same thing for the mechanical model + Torque = 3*self.pole_pairs/2 * (self.lambda_m * self.I_q + (self.L_d - self.L_q)*self.I_d*self.I_q) - T_load + + if self.theta_dot == 0 and (-1*self.b_coulomb < Torque < self.b_coulomb): + Torque = 0 + + A_m = np.add(self.A_m, np.array([[0,0], [0, -1*self.b_coulomb/self.J*sign(self.theta_dot)]])) + + SS_m = signal.cont2discrete((A_m, self.B_m, self.C_m, self.D_m),self.dT) + Ad_m = SS_m[0] # discretized A matrix for mechanical system + Bd_m = SS_m[1] + Cd_m = SS_m[2] + Dd_m = SS_m[3] + + # we now have SS models for the electrical and mechanical systems, valid at this specific time step + # update states, return as output + input_e = np.array([[V_d],[V_q_effective]]) + (I_d, I_q) = np.add(np.matmul(Ad_e, np.array([[self.I_d],[self.I_q]])), np.matmul(Bd_e, input_e)) + (theta, theta_dot) = np.add(np.matmul(Ad_m, np.array([[self.theta],[self.theta_dot]])), np.matmul(Bd_m, np.array([[Torque]]))) + + return (theta[0], theta_dot[0], I_d[0], I_q[0]) + +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) + d5065_2 = motor_pmsm_combined(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) + 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) + sol = scipy.integrate.solve_ivp(d5065_2.diff_eqs, (0,0.25), t_eval=t, args=(0,1), y0=(0,0,0,0)) + + pos = data[1] + vel = data[2] + I_d = data[3] + I_q = data[4] + + pos_ivp = sol.y[0] + vel_ivp = sol.y[1] + I_d_ivp = sol.y[2] + I_q_ivp = sol.y[3] + + fig, axs = plt.subplots(4) + + axs[0].plot(t, pos, linestyle=':', label='homebrew') + axs[0].plot(t, pos_ivp, linestyle=':', label='solve_ivp') + axs[0].set_title('pos') + axs[0].set_ylabel('Theta (eRad)') + axs[0].legend() + axs[1].plot(t, vel, linestyle=':') + axs[1].plot(t, vel_ivp, linestyle=':') + axs[1].set_title('vel') + axs[1].set_ylabel('Omega (eRad/s)') + axs[2].plot(t,I_d, linestyle=':') + axs[2].plot(t,I_d_ivp, linestyle=':') + axs[2].set_title('I_d') + axs[2].set_ylabel('Current (A)') + axs[3].plot(t,I_q, linestyle=':') + axs[3].plot(t,I_q_ivp, linestyle=':') + 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 From ead1bc3b9b1029342479f16ddcfb6beabf9328c5 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 18 Aug 2020 19:31:29 -0400 Subject: [PATCH 11/12] Changed from discrete linearization to rk_step for single-step simulation of the motor class --- analysis/Simulation/MotorSim.py | 221 ++++++++++++++++++++++---------- 1 file changed, 153 insertions(+), 68 deletions(-) diff --git a/analysis/Simulation/MotorSim.py b/analysis/Simulation/MotorSim.py index 721b9812..f57e8c51 100644 --- a/analysis/Simulation/MotorSim.py +++ b/analysis/Simulation/MotorSim.py @@ -5,6 +5,7 @@ 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: @@ -14,6 +15,73 @@ def sign(num): 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 @@ -90,7 +158,7 @@ class motor_pmsm_combined: self.R = R self.pole_pairs = pole_pairs - def diff_eqs(self, t, y, V_d, V_q): + def diff_eqs(self, t, y, V_d, V_q, T_load): # inputs are V_d, V_q # state is y, y = [theta, theta_dot, I_d, I_q] @@ -116,6 +184,7 @@ 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 @@ -136,11 +205,16 @@ class motor: self.C_e = np.array([[1,0],[0,1]]) self.D_e = np.array([[0,0],[0,0]]) + # 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] @@ -152,11 +226,7 @@ class motor: I_d = [] I_q = [] for i in range(len(t)): - out = self.singleStep(u[1],u[2],u[0]) - self.theta = out[0] - self.theta_dot = out[1] - self.I_d = out[2] - self.I_q = out[3] + self.single_step_rk(u[2],u[1],u[0]) time.append(i*self.dT) pos.append(self.theta) vel.append(self.theta_dot) @@ -165,44 +235,34 @@ class motor: return [time,pos,vel,I_d,I_q] - def singleStep(self, V_d, V_q, T_load): - # create the discretized SS electrical and mechanical models, valid for this time instance - theta_e = self.theta * self.pole_pairs - theta_dot_e = self.theta_dot * self.pole_pairs + def inputs(self, V_q, V_d, T_load): + self.V_q = V_q + self.V_d = V_d + self.T_load = T_load - V_q_effective = V_q - theta_dot_e * self.lambda_m + 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] - # make new A electrical matrix with the weird coupled theta_dot terms - A_e = np.add(self.A_e, np.array([[0, theta_dot_e * self.L_q / self.L_d],[-1*theta_dot_e * self.L_d / self.L_q, 0]])) + torque = 3*self.pole_pairs/2 * (self.lambda_m * I_q + (self.L_d - self.L_q)*I_d*I_q) - self.T_load - # SS_e is a discretized version of the electrical model, only valid for this time step (theta_dot will change) - SS_e = signal.cont2discrete((A_e, self.B_e, self.C_e, self.D_e), self.dT) - Ad_e = SS_e[0] # discretized A matrix for electrical system - Bd_e = SS_e[1] - Cd_e = SS_e[2] - Dd_e = SS_e[3] + # 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 - # do the same thing for the mechanical model - Torque = 3*self.pole_pairs/2 * (self.lambda_m * self.I_q + (self.L_d - self.L_q)*self.I_d*self.I_q) - T_load + return np.array([theta_dot, theta_ddot, I_d_dot, I_q_dot]) - if self.theta_dot == 0 and (-1*self.b_coulomb < Torque < self.b_coulomb): - Torque = 0 - - A_m = np.add(self.A_m, np.array([[0,0], [0, -1*self.b_coulomb/self.J*sign(self.theta_dot)]])) - - SS_m = signal.cont2discrete((A_m, self.B_m, self.C_m, self.D_m),self.dT) - Ad_m = SS_m[0] # discretized A matrix for mechanical system - Bd_m = SS_m[1] - Cd_m = SS_m[2] - Dd_m = SS_m[3] - - # we now have SS models for the electrical and mechanical systems, valid at this specific time step - # update states, return as output - input_e = np.array([[V_d],[V_q_effective]]) - (I_d, I_q) = np.add(np.matmul(Ad_e, np.array([[self.I_d],[self.I_q]])), np.matmul(Bd_e, input_e)) - (theta, theta_dot) = np.add(np.matmul(Ad_m, np.array([[self.theta],[self.theta_dot]])), np.matmul(Bd_m, np.array([[Torque]]))) - - return (theta[0], theta_dot[0], I_d[0], I_q[0]) + 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) @@ -211,38 +271,63 @@ if __name__ == "__main__": 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 + #start = time.time() data = d5065.simulate(t=t, u=u, x0=x0) - sol = scipy.integrate.solve_ivp(d5065_2.diff_eqs, (0,0.25), t_eval=t, args=(0,1), y0=(0,0,0,0)) + #end = time.time() + #start_ivp = time.time() + #sol = scipy.integrate.solve_ivp(d5065_2.diff_eqs, (0,0.25), t_eval=t, args=(0,1,0), y0=(0,0,0,0)) + #end_ivp = time.time() + dT = 1/48000 + states = [] + pos = [] + vel = [] + I_d = [] + I_q = [] + #d5065.inputs(V_d = 0, V_q = 1, T_load = 0) + #start = time.time() + #for i in range(12000): + # d5065.single_step_rk(V_d = 0, V_q = 1, T_load = 0) + # pos.append(d5065.theta) + # vel.append(d5065.theta_dot) + # I_d.append(d5065.I_d) + # I_q.append(d5065.I_q) + #end = time.time() + #states = [pos, vel, I_d, I_q] + #print("rk_step time") + #print(start-end) + #print("solve_ivp time") + #print(start_ivp-end_ivp) + #print("error percentage: " + str((sol.y[0][-1] - pos[-2])/ pos[-2] * 100)) - pos = data[1] - vel = data[2] - I_d = data[3] - I_q = data[4] + #pos = data[1] + #vel = data[2] + #I_d = data[3] + #I_q = data[4] - pos_ivp = sol.y[0] - vel_ivp = sol.y[1] - I_d_ivp = sol.y[2] - I_q_ivp = sol.y[3] + #pos_ivp = sol.y[0] + #vel_ivp = sol.y[1] + #I_d_ivp = sol.y[2] + #I_q_ivp = sol.y[3] - fig, axs = plt.subplots(4) +# fig, axs = plt.subplots(4) - axs[0].plot(t, pos, linestyle=':', label='homebrew') - axs[0].plot(t, pos_ivp, linestyle=':', label='solve_ivp') - axs[0].set_title('pos') - axs[0].set_ylabel('Theta (eRad)') - axs[0].legend() - axs[1].plot(t, vel, linestyle=':') - axs[1].plot(t, vel_ivp, linestyle=':') - axs[1].set_title('vel') - axs[1].set_ylabel('Omega (eRad/s)') - axs[2].plot(t,I_d, linestyle=':') - axs[2].plot(t,I_d_ivp, linestyle=':') - axs[2].set_title('I_d') - axs[2].set_ylabel('Current (A)') - axs[3].plot(t,I_q, linestyle=':') - axs[3].plot(t,I_q_ivp, linestyle=':') - axs[3].set_title('I_q') - axs[3].set_ylabel('Current (A)') - axs[3].set_xlabel('time (s)') +# axs[0].plot(t, pos, linestyle=':', label='homebrew') +# axs[0].plot(t, pos_ivp, linestyle=':', label='solve_ivp') +# axs[0].set_title('pos') +# axs[0].set_ylabel('Theta (eRad)') +# axs[0].legend() +# axs[1].plot(t, vel, linestyle=':') +# axs[1].plot(t, vel_ivp, linestyle=':') +# axs[1].set_title('vel') +# axs[1].set_ylabel('Omega (eRad/s)') +# axs[2].plot(t,I_d, linestyle=':') +# axs[2].plot(t,I_d_ivp, linestyle=':') +# axs[2].set_title('I_d') +# axs[2].set_ylabel('Current (A)') +# axs[3].plot(t,I_q, linestyle=':') +# axs[3].plot(t,I_q_ivp, linestyle=':') +# 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 +# plt.show() \ No newline at end of file From cbecd4705193b03b4f9403ddd59314a7e8884f0c Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 18 Aug 2020 19:44:21 -0400 Subject: [PATCH 12/12] Cleaned up MotorSim classes --- analysis/Simulation/MotorSim.py | 152 +++++--------------------------- 1 file changed, 22 insertions(+), 130 deletions(-) diff --git a/analysis/Simulation/MotorSim.py b/analysis/Simulation/MotorSim.py index f57e8c51..6d457db1 100644 --- a/analysis/Simulation/MotorSim.py +++ b/analysis/Simulation/MotorSim.py @@ -87,40 +87,6 @@ def rk_step(fun, t, y, f, h, A, B, C, K): # phase_L = 0.0000157 H # pole_pairs = 7 # KV = 270 - -class motor_pmsm_electrical: - # class for simulating PMSM electrical dynamics in dq reference frame - # R, L input are phase-neutral, not phase-phase - def __init__(self, R, L_q, L_d, KV, pole_pairs): - # KV is published KV - kt = 8.27/KV - self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad) - self.L_q = L_q - self.L_d = L_d - self.R = R - self.pole_pairs = pole_pairs - - self.V_q = 0 - self.V_d = 0 - self.w_e = 0 - - def diff_eqs(self, t, y, V_d, V_q, w): - # this is for solving with solve_ivp or similar - # t is time, y is vector of state: [I_d, I_q], V_d and V_q are input voltages in dq ref frame, w is electrical freq - I_d = y[0] - I_q = y[1] - - # set these equal to the inputs for plotting - self.V_d = V_d - self.V_q = V_q - self.w_e = w - - I_d_dot = V_d / self.L_d - self.R / self.L_d * I_d + w * self.L_q / self.L_d * I_q - I_q_dot = V_q / self.L_q - self.R / self.L_q * I_q - w * self.L_d / self.L_q * I_d - w * self.lambda_m / self.L_q - - return np.array([I_d_dot, I_q_dot]) - -# example params for D5065 motor # J = 1e-4 # b_coulomb = 0.001 # b_viscous = 0.001 @@ -142,40 +108,6 @@ class motor_pmsm_mechanical: return np.array([theta_dot, theta_ddot]) -class motor_pmsm_combined: - def __init__(self, J, b_coulomb, b_viscous, R, L_q, L_d, KV, pole_pairs): - # 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 - - kt = 8.27/KV - self.lambda_m = 2*kt/(3*pole_pairs) #speed constant in Vs/rad (electrical rad) - self.L_q = L_q - self.L_d = L_d - self.R = R - self.pole_pairs = pole_pairs - - def diff_eqs(self, t, y, V_d, V_q, T_load): - # inputs are V_d, V_q - # state is y, y = [theta, theta_dot, I_d, I_q] - - 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) - - # theta_dot = theta_dot, no ode here - theta_ddot = (1/self.J) * (torque - self.b_v * theta_dot - self.b_c * sign(theta_dot)) - I_d_dot = 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 = 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 inverter(vbus, timings, current): # this function should take the relevant inputs and output voltages in dq reference frame. pass @@ -193,17 +125,6 @@ class motor: self.L_q = L_q self.L_d = L_d self.J = J - # set up SS matrices - # _m for mechanical, _e for electrical - self.A_m = np.array([[0,1],[0,-1*b_viscous/J]]) - self.B_m = np.array([[0],[1/J]]) - self.C_m = np.array([[1,0],[0,1]]) - self.D_m = np.array([[0],[0]]) - - self.A_e = np.array([[-1*R/L_d, 0],[0, -1*R/L_q]]) # the zero terms get replaced by the theta_dot terms in simulate - self.B_e = np.array([[1/L_d, 0],[0, 1/L_q]]) - self.C_e = np.array([[1,0],[0,1]]) - self.D_e = np.array([[0,0],[0,0]]) # state variables for motor self.theta = 0 # mechanical! @@ -251,6 +172,9 @@ class motor: 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 @@ -266,68 +190,36 @@ class motor: 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) - d5065_2 = motor_pmsm_combined(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) 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 - #start = time.time() data = d5065.simulate(t=t, u=u, x0=x0) - #end = time.time() - #start_ivp = time.time() - #sol = scipy.integrate.solve_ivp(d5065_2.diff_eqs, (0,0.25), t_eval=t, args=(0,1,0), y0=(0,0,0,0)) - #end_ivp = time.time() dT = 1/48000 states = [] pos = [] vel = [] I_d = [] I_q = [] - #d5065.inputs(V_d = 0, V_q = 1, T_load = 0) - #start = time.time() - #for i in range(12000): - # d5065.single_step_rk(V_d = 0, V_q = 1, T_load = 0) - # pos.append(d5065.theta) - # vel.append(d5065.theta_dot) - # I_d.append(d5065.I_d) - # I_q.append(d5065.I_q) - #end = time.time() - #states = [pos, vel, I_d, I_q] - #print("rk_step time") - #print(start-end) - #print("solve_ivp time") - #print(start_ivp-end_ivp) - #print("error percentage: " + str((sol.y[0][-1] - pos[-2])/ pos[-2] * 100)) + pos = data[1] + vel = data[2] + I_d = data[3] + I_q = data[4] - #pos = data[1] - #vel = data[2] - #I_d = data[3] - #I_q = data[4] + fig, axs = plt.subplots(4) - #pos_ivp = sol.y[0] - #vel_ivp = sol.y[1] - #I_d_ivp = sol.y[2] - #I_q_ivp = sol.y[3] + 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)') -# fig, axs = plt.subplots(4) - -# axs[0].plot(t, pos, linestyle=':', label='homebrew') -# axs[0].plot(t, pos_ivp, linestyle=':', label='solve_ivp') -# axs[0].set_title('pos') -# axs[0].set_ylabel('Theta (eRad)') -# axs[0].legend() -# axs[1].plot(t, vel, linestyle=':') -# axs[1].plot(t, vel_ivp, linestyle=':') -# axs[1].set_title('vel') -# axs[1].set_ylabel('Omega (eRad/s)') -# axs[2].plot(t,I_d, linestyle=':') -# axs[2].plot(t,I_d_ivp, linestyle=':') -# axs[2].set_title('I_d') -# axs[2].set_ylabel('Current (A)') -# axs[3].plot(t,I_q, linestyle=':') -# axs[3].plot(t,I_q_ivp, linestyle=':') -# 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 + plt.show() \ No newline at end of file