Issue #16: Get rid of hysteresis in MotorController::SetMotorRPM

Author: AksonovSergei

src/kpi_rover_ecu/include/protocolHandler.h

@@ -22,7 +22,7 @@ class ProtocolHanlder {
     std::vector<uint8_t> MotorsStopMessage();
 
    private:
-    MotorController* main_controller_;
+    MotorController* motors_controller_;
     std::vector<uint8_t> HandleSetMotorSpeed(const std::vector<uint8_t>& message);
     std::vector<uint8_t> HandleGetApiVersion(const std::vector<uint8_t>& message);
     std::vector<uint8_t> HandleSetAllMotorsSpeed(const std::vector<uint8_t>& message);

src/kpi_rover_ecu/src/motorsController.cpp

@@ -47,29 +47,22 @@ int MotorController::Init(const std::vector<MotorConfig>& _motors, uint8_t _moto
 }
 
 int MotorController::SetMotorRPM(int channel, int newRPM) {
-    if (std::abs(newRPM) > Motor::kMinRpm) {
-        if (newRPM >= Motor::kMaxRpm) {
-            std::cout << "[INFO][RC] Set RPM to max value" << '\n';
-            if (motors_[channel].MotorGo(Motor::kMaxRpm) != 0) {
-                std::cout << "[ERROR][RC] Error while set new RPM" << '\n';
-                return -1;
-            }
-        } else {
-            if (motors_[channel].MotorGo(newRPM) != 0) {
-                std::cout << "[ERROR][RC] Error while set new RPM" << '\n';
-                return -1;
-            }
-        }
-    } else {
-        if (std::abs(newRPM) != 0) {
-            std::cout << "[INFO][RC] Set RPM to stop because RPM less than MIN_RPM" << '\n';
-        }
-
-        if (motors_[channel].MotorStop() != 0) {
-            std::cout << "[ERROR][RC] Error while set new RPM" << '\n';
-            return -1;
-        }
+    if (channel >= motor_number_) {
+        std::cout << "[ERROR][RC] Channel out of range" << '\n';
+        return -1;
+    }
+
+    if (std::abs(newRPM) > Motor::kMaxRpm) {
+        std::cout << "[WARNING][RC] RPM out of range" << '\n';
+        newRPM = Motor::kMaxRpm;
     }
+
+    int res = motors_[channel].MotorGo(newRPM);
+    if (res != 0) {
+        std::cout << "[ERROR][RC] Error while set new RPM" << '\n';
+        return -1;
+    }
+
     return 0;
 }
 

src/kpi_rover_ecu/src/protocolHandler.cpp

@@ -18,7 +18,7 @@ constexpr uint8_t ProtocolHanlder::kIdSetAllMotorsSpeed;
 constexpr uint8_t ProtocolHanlder::kIdGetEncoder;
 constexpr uint8_t ProtocolHanlder::kIdGetAllEncoders;
 
-ProtocolHanlder::ProtocolHanlder(MotorController* motorDriver) : main_controller_(motorDriver) {}
+ProtocolHanlder::ProtocolHanlder(MotorController* motorDriver) : motors_controller_(motorDriver) {}
 
 vector<uint8_t> ProtocolHanlder::HandleSetMotorSpeed(const vector<uint8_t>& message) {
     const uint8_t kMotorId = message[1];
@@ -32,7 +32,7 @@ vector<uint8_t> ProtocolHanlder::HandleSetMotorSpeed(const vector<uint8_t>& mess
               << '\n';
     std::cout << "[INFO] Motor set run" << '\n';
 
-    if (main_controller_->SetMotorRPM(static_cast<int>(kMotorId), static_cast<int>(k_motor_rpm)) != 0) {
+    if (motors_controller_->SetMotorRPM(static_cast<int>(kMotorId), static_cast<int>(k_motor_rpm)) != 0) {
         std::cout << "[ERROR] Error setMotorRPM, retry connection" << '\n';
         return {};
     }
@@ -58,20 +58,20 @@ vector<uint8_t> ProtocolHanlder::HandleGetApiVersion(const vector<uint8_t>& mess
 }
 
 vector<uint8_t> ProtocolHanlder::HandleSetAllMotorsSpeed(const vector<uint8_t>& message) {
-    std::vector<int32_t> motors_rpm_arr(main_controller_->GetMotorsNumber(), 0);
+    std::vector<int32_t> motors_rpm_arr(motors_controller_->GetMotorsNumber(), 0);
     vector<uint8_t> ret_val;
 
-    for (int i = 0; i < main_controller_->GetMotorsNumber(); i++) {
+    for (int i = 0; i < motors_controller_->GetMotorsNumber(); i++) {
         memcpy(&motors_rpm_arr[i], &message[1 + i * sizeof(int32_t)], sizeof(int32_t));
         motors_rpm_arr[i] = static_cast<int32_t>(ntohl(motors_rpm_arr[i]));
     }
 
-    for (int i = 0; i < main_controller_->GetMotorsNumber(); i++) {
+    for (int i = 0; i < motors_controller_->GetMotorsNumber(); i++) {
         if (motors_rpm_arr[i] != 0) {
             std::cout << "[COMMAND] motor " << i << " new rpm " << static_cast<int>(motors_rpm_arr[i]) << '\n';
         }
 
-        if (main_controller_->SetMotorRPM(i, static_cast<int>(motors_rpm_arr[i])) != 0) {
+        if (motors_controller_->SetMotorRPM(i, static_cast<int>(motors_rpm_arr[i])) != 0) {
             std::cout << "[ERROR] Error setMotorRPM, retry connection" << '\n';
             return {};
         }
@@ -86,7 +86,7 @@ vector<uint8_t> ProtocolHanlder::HandleGetEncoder(const vector<uint8_t>& message
     vector<uint8_t> ret_val;
     std::cout << "[COMMAND] get motor " << static_cast<int>(kMotorId) << " encoder " << '\n';
 
-    const int32_t kMotorRpm = main_controller_->GetEncoderCounter(kMotorId);
+    const int32_t kMotorRpm = motors_controller_->GetEncoderCounter(kMotorId);
     ret_val.push_back(ProtocolHanlder::kIdGetEncoder);
 
     auto k_motor_rpm_order = static_cast<int32_t>(htonl(kMotorRpm));
@@ -103,8 +103,8 @@ vector<uint8_t> ProtocolHanlder::HandleGetAllEncoders(const vector<uint8_t>& mes
     ret_val.push_back(ProtocolHanlder::kIdGetAllEncoders);
 
     uint8_t buffer[sizeof(int32_t)];
-    for (int i = 0; i < main_controller_->GetMotorsNumber(); i++) {
-        auto encoder_rpm = static_cast<int32_t>(htonl(main_controller_->GetEncoderCounter(i)));
+    for (int i = 0; i < motors_controller_->GetMotorsNumber(); i++) {
+        auto encoder_rpm = static_cast<int32_t>(htonl(motors_controller_->GetEncoderCounter(i)));
         std::memcpy(buffer, &encoder_rpm, sizeof(int32_t));
         ret_val.insert(ret_val.end(), buffer, buffer + sizeof(int32_t));
     }
@@ -144,7 +144,7 @@ vector<uint8_t> ProtocolHanlder::MotorsStopMessage() {
     int32_t stop_value = 0;
     ret_val.push_back(ProtocolHanlder::kIdSetAllMotorsSpeed);
     uint8_t buffer[sizeof(int32_t)];
-    for (int i = 0; i < main_controller_->GetMotorsNumber(); i++) {
+    for (int i = 0; i < motors_controller_->GetMotorsNumber(); i++) {
         std::memcpy(buffer, &stop_value, sizeof(int32_t));
         ret_val.insert(ret_val.end(), buffer, buffer + sizeof(int32_t));
     }