ODrive receives coupled positions in rad

Nate711 · Aug 23, 2018 · 7feb7ff · 7feb7ff
1 parent 7d69747
commit 7feb7ff
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 3 deletions.
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
@@ -45,14 +45,18 @@ void Controller::set_current_setpoint(float current_setpoint) {
 }
 
 void Controller::set_coupled_setpoints(float theta_setpoint, float gamma_setpoint) {
-    theta_setpoint_ = theta_setpoint;
-    gamma_setpoint_ = gamma_setpoint;
+    theta_setpoint_ = rad_to_encoder(theta_setpoint);
+    gamma_setpoint_ = rad_to_encoder(gamma_setpoint);
     config_.control_mode = CTRL_MODE_COUPLED_CONTROL;
 #ifdef DEBUG_PRINT
     printf("COUPLED_CONTROL %3.3f %3.3f\n", theta_setpoint_, gamma_setpoint_);
 #endif
 }
 
+float rad_to_encoder(float theta) {
+    return theta * axis_->encoder_.config_.cpr * config_.gear_ratio / (2 * M_PI);
+}
+
 void Controller::start_anticogging_calibration() {
     // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
     if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) {

diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
@@ -27,6 +27,8 @@ struct ControllerConfig_t {
     float kd_theta = 5.0f / 10000.0f;
     float kp_gamma = 15.0f;
     float kd_gamma = 5.0f / 10000.0f;
+
+    float gear_ratio = 3.0;
 };
 
 class Controller {
@@ -39,6 +41,8 @@ class Controller {
     void set_current_setpoint(float current_setpoint);
     void set_coupled_setpoints(float theta_setpoint, float gamma_setpoint);
 
+    float rad_to_encoder(float theta);
+
     // TODO: make this more similar to other calibration loops
     void start_anticogging_calibration();
     bool anticogging_calibration(float pos_estimate, float vel_estimate);

diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp
@@ -210,6 +210,11 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         float theta_sp, gamma_sp;
         int result = parseDualCurrent(cmd,theta_sp, gamma_sp);
 
+        const float MULTIPLIER = 1000.0;
+
+        theta_sp /= MULTIPLIER;
+        gamma_sp /= MULTIPLIER;
+
         if (result != 1) {
             respond(response_channel, use_checksum, "Failed on parse or checksum: ");
             respond(response_channel, use_checksum, cmd);

diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default
@@ -1,6 +1,6 @@
 # Copy this file to tup.config and adapt it to your needs
 # make sure this fits your board
-#CONFIG_BOARD_VERSION=v3.5-24V
+#CONFIG_BOARD_VERSION=v3.5-48V
 CONFIG_USB_PROTOCOL=native
 CONFIG_UART_PROTOCOL=ascii