request multi-turn position until motors respond

zauberzeug · Jun 14, 2023 · a89c2ef · a89c2ef
1 parent e663672
commit a89c2ef
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git a/main/modules/rmd_motor.cpp b/main/modules/rmd_motor.cpp
@@ -16,7 +16,6 @@ RmdMotor::RmdMotor(const std::string name, const Can_ptr can, const uint8_t moto
 
 void RmdMotor::subscribe_to_can() {
     can->subscribe(this->motor_id + 0x240, std::static_pointer_cast<Module>(this->shared_from_this()));
-    this->send(0x92, 0, 0, 0, 0, 0, 0, 0);
 }
 
 void RmdMotor::send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3,
@@ -41,6 +40,10 @@ void RmdMotor::send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const
 void RmdMotor::step() {
     this->properties.at("can_age")->number_value = millis_since(this->last_msg_millis) / 1e3;
 
+    if (!this->has_last_encoder_position) {
+        this->send(0x92, 0, 0, 0, 0, 0, 0, 0);
+    }
+
     this->send(0x9c, 0, 0, 0, 0, 0, 0, 0);
     Module::step();
 }
@@ -164,6 +167,7 @@ void RmdMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t
         while (this->last_encoder_position < -65536 * 2 / this->ratio) {
             this->last_encoder_position += 65536 * 4 / this->ratio;
         }
+        this->has_last_encoder_position = true;
         break;
     }
     case 0x9c: {

diff --git a/main/modules/rmd_motor.h b/main/modules/rmd_motor.h
@@ -15,6 +15,7 @@ class RmdMotor : public Module, public std::enable_shared_from_this<RmdMotor> {
     uint8_t last_msg_id = 0;
     int ratio;
     int32_t last_encoder_position;
+    bool has_last_encoder_position = false;
     unsigned long int last_msg_millis = 0;
 
     void send(const uint8_t d0, const uint8_t d1, const uint8_t d2, const uint8_t d3,