Make brushed motors resistant to changing mission (#474)

--------
Co-authored-by: wisabel0 <wisabel@umich.edu>
Co-authored-by: tabiosg <gutstabios@gmail.com>

src/esw/brushed_motors/Controller.cpp

@@ -1,5 +1,8 @@
 #include "Controller.h"
 
+std::unordered_map<uint8_t, std::string> Controller::liveMap;
+std::mutex Controller::liveMapLock;
+
 // REQUIRES: _name is the name of the motor,
 // mcu_id is the mcu id of the controller which dictates the slave address,
 // motor_id is the motor id of the motor that is to be controlled,
@@ -22,6 +25,7 @@ Controller::Controller(
     assert(0.0f < _motorMaxVoltage);
     assert(_motorMaxVoltage <= _driverVoltage);
     assert(_driverVoltage <= 36.0f);
+    assert((motorID & 0b111) == motorID);
     name = _name;
     deviceAddress = mcuID;
     motorID = _motorID;
@@ -35,6 +39,7 @@ Controller::Controller(
     limit_switch_data.limit_b_pressed = false;
 }
 
+
 // REQUIRES: nothing
 // MODIFIES: nothing
 // EFFECTS: Returns last saved value of angle.
@@ -62,13 +67,6 @@ void Controller::overrideCurrentAngle(float newAngleRad) {
     }
 }
 
-// REQUIRES: nothing
-// MODIFIES: nothing
-// EFFECTS: Returns true if Controller is live.
-bool Controller::isControllerLive() const {
-    return isLive;
-}
-
 // REQUIRES: -1.0 <= input <= 1.0
 // MODIFIES: currentAngle. Also makes controller live if not already.
 // EFFECTS: I2C bus, Sends an open loop command scaled to PWM limits
@@ -296,8 +294,9 @@ void Controller::turnOn() const {
 }
 
 // REQUIRES: nothing
-// MODIFIES: nothing
-// EFFECTS: UART bus, and turns on the controller. Can be used as a way to tick the watchdog for a particular mcu.
+// MODIFIES: liveMap
+// EFFECTS: UART bus, and turns on the controller.
+// Can be used as a way to tick the watchdog for a particular mcu.
 void Controller::turnOnViaUART() const {
     try {
         UART::transact(deviceAddress, motorIDRegMask | ON_OP, ON_WB,
@@ -307,13 +306,34 @@ void Controller::turnOnViaUART() const {
     }
 }
 
+// REQUIRES: nothing
+// MODIFIES: nothing
+// EFFECTS: Returns a combined ID for both the deviceAddress and motorID
+// MotorID can only be max 3 bits (0-5), and device address is max 2 bits (1 or 2)
+uint8_t Controller::combineDeviceMotorID() const {
+    return (deviceAddress << 3) | motorID;
+}
+
 // REQUIRES: nothing
 // MODIFIES: isLive
 // EFFECTS: I2C bus, if not already live,
 // configures the physical controller.
 // Then makes live.
 void Controller::makeLive() {
-    if (isLive) {
+    std::unique_lock<std::mutex>
+            lock(liveMapLock);
+
+    uint8_t key = combineDeviceMotorID();
+
+    // if key is absent, no motor with that key has been made live
+    auto it = liveMap.find(key);
+    if (it == liveMap.end()) {
+        // Map entry starts with an empty string until that joint is live
+        it = liveMap.emplace(key, "").first;
+    }
+
+    // already live and configured to correct motor
+    if (it->second == name) {
         return;
     }
 
@@ -367,7 +387,8 @@ void Controller::makeLive() {
         I2C::transact(deviceAddress, motorIDRegMask | ENABLE_LIMIT_B_OP, ENABLE_LIMIT_WB,
                       ENABLE_LIMIT_RB, buffer, nullptr);
 
-        isLive = true;
+        // update liveMap
+        it->second = name;
 
     } catch (IOFailure& e) {
         ROS_ERROR("makeLive failed on %s", name.c_str());
@@ -376,12 +397,25 @@ void Controller::makeLive() {
 }
 
 // REQUIRES: nothing
-// MODIFIES: isLive
+// MODIFIES: liveMap
 // EFFECTS: UART bus, if not already live,
 // configures the physical controller.
 // Then makes live.
 void Controller::makeLiveViaUART() {
-    if (isLive) {
+    std::unique_lock<std::mutex>
+            lock(liveMapLock);
+
+    uint8_t key = combineDeviceMotorID();
+
+    // if key is absent, no motor with that key has been made live
+    auto it = liveMap.find(key);
+    if (it == liveMap.end()) {
+        // Map entry starts with an empty string until that joint is live
+        it = liveMap.emplace(key, "").first;
+    }
+
+    // already live and configured to correct motor
+    if (it->second == name) {
         return;
     }
 
@@ -444,7 +478,8 @@ void Controller::makeLiveViaUART() {
         UART::transact(deviceAddress, motorIDRegMask | ENABLE_LIMIT_B_OP, ENABLE_LIMIT_WB,
                        buffer);
 
-        isLive = true;
+        // update liveMap
+        it->second = name;
 
     } catch (IOFailure& e) {
         ROS_ERROR("makeLiveViaUART failed on %s", name.c_str());

src/esw/brushed_motors/Controller.h

@@ -102,6 +102,11 @@ sent. This is to prevent multiple virtual Controller objects from
 trying to contact the same physical Controller object.)
 */
 
+// struct LiveState {
+//     bool isLive{false};
+//     std::string jointName;
+//     inline static std::mutex liveMutex;
+// };
 
 class Controller {
 public:
@@ -121,6 +126,7 @@ class Controller {
     int32_t limitAAdjustedCounts = 0;
     int32_t limitBAdjustedCounts = 0;
 
+
     // REQUIRES: _name is the name of the motor,
     // mcuID is the mcu id of the controller which dictates the slave address,
     // _motorID is the motor id of the motor that is to be controlled,
@@ -141,11 +147,6 @@ class Controller {
             float _motorMaxVoltage,
             float _driverVoltage);
 
-    // REQUIRES: nothing
-    // MODIFIES: nothing
-    // EFFECTS: Returns true if Controller is live.
-    bool isControllerLive() const;
-
     // REQUIRES: nothing
     // MODIFIES: nothing
     // EFFECTS: Returns last saved value of angle.
@@ -205,21 +206,28 @@ class Controller {
     void turnOn() const;
 
     // REQUIRES: nothing
-    // MODIFIES: nothing
-    // EFFECTS: UART bus, and turns on the controller. Can be used as a way to tick the watchdog for a particular mcu.
+    // MODIFIES: liveMap
+    // EFFECTS: UART bus, and turns on the controller.
+    // Can be used as a way to tick the watchdog for a particular mcu.
     void turnOnViaUART() const;
 
+    // REQUIRES: nothing
+    // MODIFIES: nothing
+    // EFFECTS: Returns a combined ID for both the deviceAddress and motorID
+    // MotorID can only be max 3 bits (0-5), and device address is max 2 bits (1 or 2)
+    uint8_t combineDeviceMotorID() const;
+
 
 private:
     // REQUIRES: nothing
-    // MODIFIES: isLive
+    // MODIFIES: liveMap
     // EFFECTS: I2C bus, If not already live,
     // configures the physical controller.
     // Then makes live.
     void makeLive();
 
     // REQUIRES: nothing
-    // MODIFIES: isLive
+    // MODIFIES: liveMap
     // EFFECTS: UART bus, If not already live,
     // configures the physical controller.
     // Then makes live.
@@ -244,8 +252,12 @@ class Controller {
 
     float currentAngle;
 
-    bool isLive = false;
+    // key is deviceAddress and motorID (eg. if deviceAddress = 2(0b10) and motorID = 1(0b1), then key = 17(0b10001) )
+    static std::unordered_map<uint8_t, std::string> liveMap;
+    static std::mutex liveMapLock;
+
     bool isControllerCalibrated = false;
+
     float abs_enc_radians = 0;
     mrover::LimitSwitchData limit_switch_data;
 };