Broken code

Author: imisaacwu

src/CAN/CAN.cpp

@@ -43,6 +43,7 @@ namespace {
 // time to sleep after getting a CAN read error
 constexpr std::chrono::milliseconds READ_ERR_SLEEP(100);
 constexpr std::chrono::milliseconds ACK_TIMEOUT(50);
+constexpr std::chrono::milliseconds READ_TIMEOUT(100);
 // CAN26 11-bit ID layout: [priority:1][deviceUUID:7][peripheral:1][power:1][motor:1]
 // Match on UUID field to filter for packets addressed to this device
 constexpr uint32_t CAN_MASK = 0x3F8; // UUID field
@@ -66,7 +67,7 @@ std::unordered_map<
 std::shared_mutex directReadMapMutex;
 std::unordered_map<
 	std::pair<CANDeviceUUID_t, uint16_t>,
-	std::function<void(CANMotorPacket_BLDC_DirectReadResult_Decoded_t)>> directReadCallbackMap;
+	std::pair<std::function<void(CANMotorPacket_BLDC_DirectReadResult_Decoded_t)>, std::thread>> directReadCallbackMap;
 
 // Endpoint JSONs
 nlohmann::json pro_endpoints;
@@ -133,7 +134,7 @@ void handleDirectRead(CANPacket_t& packet) {
 	// Unlock in case callback wants to modify the map?
 	mapReadLock.unlock();
 	if (it != directReadCallbackMap.end()) {
-		it->second(decoded);
+		it->second.first(decoded);
 	}
 }
 
@@ -362,7 +363,17 @@ void addDirectReadCallback(CANDevice_t device, uint16_t endpoint, const std::fun
 		LOG_F(WARNING, "Callback already exists for 0x%x endpoint %d! Ignoring..", device.deviceUUID, endpoint);
 		return;
 	}
-	directReadCallbackMap.insert({key, callback});
+	std::thread timeout([device, endpoint]() {
+		std::this_thread::sleep_for(READ_TIMEOUT);
+		LOG_F(ERROR, "0x%x read of %d timed out!", device.deviceUUID, endpoint);
+	});
+	auto element = std::make_pair<const std::function<void (CANMotorPacket_BLDC_DirectReadResult_Decoded_t)>, std::thread>(callback, timeout);
+
+	// std::unordered_map<
+	// 	std::pair<CANDeviceUUID_t, uint16_t>,
+	// 	std::pair<std::function<void(CANMotorPacket_BLDC_DirectReadResult_Decoded_t)>, std::thread>> directReadCallbackMap;
+
+	directReadCallbackMap.insert({key, element});
 }
 
 void removeDirectReadCallback(CANDevice_t device, uint16_t endpoint) {

src/CAN/CANBoard.cpp

@@ -47,6 +47,13 @@ CANBoard::CANBoard(robot::types::boardid_t board_id, CANDevice_t device)
         if (auto it = robot::boardInversionMap.find(board_id); it != robot::boardInversionMap.end()) {
             this->inversion_factor = it->second;
         }
+
+        // Wait until configs are grabbed
+        auto start = std::chrono::system_clock::now();
+        while (!this->vel_limit);
+        auto end = std::chrono::system_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
+        LOG_F(INFO, "0x%x took %ld ms", this->device.deviceUUID, elapsed.count());
     }
 
     if (device.peripheralDomain) {
@@ -70,24 +77,21 @@ void CANBoard::setMotorPower(double power) {
         if (this->watchdog) {
             this->setMotorState(can::motor::axis_state_t::idle);
 
-            // hang until this actually goes idle for funsies
-            this->correct = false;
             if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.current_state"); endpoint != nullptr) {
                 uint16_t endpoint_id = endpoint["id"];
                 addDirectReadCallback(this->device, endpoint_id, [this, endpoint_id](auto decoded) {
                     if (decoded.value_uint8 != static_cast<uint8_t>(can::motor::axis_state_t::idle)) {
                         LOG_F(ERROR, "0x%x DID NOT LISTEN AND IS NOT IDLE", this->device.deviceUUID);
                         this->setMotorState(can::motor::axis_state_t::idle);
+                        this->read(endpoint_id);
                     } else {
                         removeDirectReadCallback(this->device, endpoint_id);
-                        correct = true;
                     }
                 });
 
                 this->read(endpoint_id);
             }
 
-            while (!this->correct);
         } else if (this->board_id == robot::types::boardid_t::shoulder || this->board_id == robot::types::boardid_t::elbow) {
             // Set motor general lockin vel to 0
             this->setMotorState(can::motor::axis_state_t::lockin_spin);

src/CAN/CANBoard.h

@@ -45,9 +45,6 @@ class CANBoard {
     // Estimates received
     std::shared_mutex board_mutex;
     robot::types::DataPoint<int32_t> position_mdeg;
-
-    // debug
-    bool correct = false;
 };
 
 } // namespace can

src/CAN/FakeCANBoard.cpp

@@ -61,9 +61,10 @@ int main() {
 
 		while (true) {
 			if (testMode == TestMode::State) {
-				std::stringstream state_msg("Enter desired motor state:\n");
-				state_msg << static_cast<uint8_t>(can::motor::axis_state_t::idle) << " idle\n";
-				state_msg << static_cast<uint8_t>(can::motor::axis_state_t::closed_loop_control) << " closed loop control\n";
+				std::stringstream state_msg("");
+				state_msg << "Enter desired motor state:\n";
+				state_msg << static_cast<int>(can::motor::axis_state_t::idle) << " idle\n";
+				state_msg << static_cast<int>(can::motor::axis_state_t::closed_loop_control) << " closed loop control\n";
 
 				int state = prompt(state_msg.str().c_str());
 				can::motor::axis_state_t motor_state = static_cast<can::motor::axis_state_t>(state);
@@ -73,9 +74,10 @@ int main() {
 				if (nlohmann::json endpoint = can::getEndpoint(board->getBoardID(), "axis0.current_state"); endpoint != nullptr) {
 					uint16_t endpoint_id = endpoint["id"];
 					can::addDirectReadCallback(board->getDevice(), endpoint_id, [board, motor_state, endpoint_id](auto decoded) {
-						if (decoded.value_uint8 != static_cast<uint8_t>(can::motor::axis_state_t::idle)) {
-							LOG_F(ERROR, "0x%x DID NOT LISTEN AND IS NOT STATE %d", board->getDevice().deviceUUID, motor_state);
+						if (decoded.value_uint8 != static_cast<uint8_t>(motor_state)) {
+							LOG_F(ERROR, "0x%x DID NOT LISTEN AND IS NOT STATE %d", board->getDevice().deviceUUID, static_cast<uint8_t>(motor_state));
 							board->setMotorState(motor_state);
+							board->read(endpoint_id);
 						} else {
 							can::removeDirectReadCallback(board->getDevice(), endpoint_id);
 							correct = true;