Fix problems

Author: imisaacwu

src/CAN/CAN.cpp

@@ -168,14 +168,6 @@ void handleHeartbeatPacket(CANPacket_t& packet) {
 	}
 }
 
-void handleLimitSwitchAlert(CANPacket_t& packet) {
-	auto decoded = CANMotorPacket_LimitSwitchAlert_Decode(&packet);
-	CANDeviceUUID_t uuid = packet.senderUUID;
-	telemetrycode_t telemCode = static_cast<telemetrycode_t>(telemtype_t::limit_switch);
-	storeTelemetry(uuid, telemCode,
-				   robot::types::DataPoint<telemetry_t>(static_cast<telemetry_t>(decoded.switchStatus)));
-}
-
 // returns a file descriptor, or -1 on failure
 int createCANSocket(std::optional<CANDevice_t> device) {
 	int fd;
@@ -227,6 +219,15 @@ int createCANSocket(std::optional<CANDevice_t> device) {
 	return fd;
 }
 
+bool sendCANFrame(const canfd_frame& frame) {
+	std::lock_guard lock(socketMutex);
+	// note that frame is a canfd_frame but we're using sizeof(can_frame)
+	// not sure why this is required to work
+	bool success = write(can_fd, &frame, sizeof(struct can_frame)) == sizeof(struct can_frame);
+	tcdrain(can_fd);
+	return success;
+}
+
 void receiveThreadFn() {
 	loguru::set_thread_name("CAN_Receive");
 	CANPacket_t packet;
@@ -256,7 +257,7 @@ void receiveThreadFn() {
 					break;
 
 				case CAN_COMMAND_ID__LIMIT_SWITCH_ALERT:
-					handleLimitSwitchAlert(packet);
+					// handleLimitSwitchAlert(packet);
 					break;
 
 				case CAN_COMMAND_ID__BLDC_DIRECT_READ_RESULT:
@@ -336,15 +337,6 @@ void sendCANPacket(const CANPacket_t& packet) {
 	}
 }
 
-bool sendCANFrame(const canfd_frame& frame) {
-	std::lock_guard lock(socketMutex);
-	// note that frame is a canfd_frame but we're using sizeof(can_frame)
-	// not sure why this is required to work
-	bool success = write(can_fd, &frame, sizeof(struct can_frame)) == sizeof(struct can_frame);
-	tcdrain(can_fd);
-	return success;
-}
-
 void printCANPacket(const CANPacket_t& packet) {
 	CANPacket_t mutablePacket = packet; // same as sendCANPacket
 	std::stringstream ss;
@@ -362,28 +354,6 @@ void printCANPacket(const CANPacket_t& packet) {
 	LOG_F(INFO, ss.str().c_str());
 }
 
-robot::types::DataPoint<telemetry_t> getDeviceTelemetry(CANDeviceUUID_t uuid,
-														telemtype_t telemType) {
-	std::shared_lock mapLock(telemMapMutex); // acquire read lock
-	// find entry for device in map
-	auto entry = telemMap.find(uuid);
-	if (entry != telemMap.end()) {
-		auto& devMutex = *entry->second.first;
-		auto& devMap = *entry->second.second;
-		// acquire read lock of device map
-		std::shared_lock deviceLock(devMutex);
-		// find entry for telemetry
-		auto telemEntry = devMap.find(static_cast<telemetrycode_t>(telemType));
-		if (telemEntry != devMap.end()) {
-			return telemEntry->second;
-		} else {
-			return {};
-		}
-	} else {
-		return {};
-	}
-}
-
 void addDirectReadCallback(CANDevice_t device, uint16_t endpoint, const std::function<void(CANMotorPacket_BLDC_DirectReadResult_Decoded_t)>& callback) {
 	auto key = std::make_pair(static_cast<uint8_t>(device.deviceUUID), endpoint);
 	// Write access

src/CAN/CAN.h

@@ -87,19 +87,6 @@ void sendCANPacket(const CANPacket_t& packet);
  */
 void printCANPacket(const CANPacket_t& packet);
 
-/**
- * @brief Get the latest telemetry from a CAN device.
- *
- * This method does NOT query for new data, it just returns the last reported value.
- *
- * @param id The device group and serial number of the device.
- * @param telemType The type of telemetry to get, as dictated by the specific device specs.
- * @return robot::types::DataPoint<telemetry_t> The telemetry value, with the timestamp of when
- * it was received. If no data is available for the given telemetry type, an empty data point
- * is returned.
- */
-robot::types::DataPoint<telemetry_t> getDeviceTelemetry(uuid_t uuid, telemtype_t telemType);
-
 /**
  * @brief Add a callback to run when we receive a read result packet corresponding
  * to the input endpoint. Callbacks persist until they are manually removed using

src/CAN/CANBoard.cpp

@@ -67,7 +67,7 @@ void CANBoard::setMotorPower(double power) {
     this->setMotorState(can::motor::axis_state_t::closed_loop_control);
 
     if (power == 0.0) {
-        if (static_cast<uint8_t>(this->board_id) < 5) {   // hack for wheels + base
+        if (this->watchdog) {
             this->setMotorState(can::motor::axis_state_t::idle);
 
             // hang until this actually goes idle for funsies
@@ -139,9 +139,11 @@ void CANBoard::setMotorVel(int8_t velocity) {
         return;
     }
 
+    float rot_vel = velocity / Constants::MILLIDEGREES_PER_REV;
+
     // Make CANPacket_t
     CANPacket_t p = CANMotorPacket_BLDC_SetInputVelocity(
-        Constants::JETSON_DEVICE, this->device, velocity, 0.0f
+        Constants::JETSON_DEVICE, this->device, rot_vel, 0.0f
     );
 
     // Send packet

src/CAN/CANBoard.h

@@ -1,7 +1,10 @@
 #pragma once
 
-#include "../world_interface/data.h"
 #include "CAN.h"
+#include "../world_interface/data.h"
+
+#include <mutex>
+#include <shared_mutex>
 
 namespace can {
 
@@ -19,13 +22,14 @@ class CANBoard {
 
     robot::types::boardid_t getBoardID() const { return board_id; }
     CANDevice_t getDevice() const { return device; }
-    robot::types::DataPoint<int32_t> getPosition() const {
+
+    robot::types::DataPoint<int32_t> getPosition() {
       std::shared_lock lock(board_mutex);
       return this->position_mdeg;
     }
 
-    void storePosition(robot::types::DataPoint<int32_t> data) const {
-      std::lock_guard lock(board_mutex);
+    void storePosition(const robot::types::DataPoint<int32_t> data) {
+      std::unique_lock lock(board_mutex);
       this->position_mdeg = data;
     }
 
@@ -39,7 +43,7 @@ class CANBoard {
     bool watchdog;
 
     // Estimates received
-    std::mutex board_mutex;
+    std::shared_mutex board_mutex;
     robot::types::DataPoint<int32_t> position_mdeg;
 
     // debug

src/CAN/CANUtils.h

@@ -104,7 +104,7 @@ struct packettype_t {
 using deviceserial_t = uint8_t;
 
 /** @brief The type of telemetry data. */
-using telemetry_t = int32_t;
+// using telemetry_t = int32_t;
 
 /**
  * @brief A unique identifier for a CAN device.

src/CMakeLists.txt

@@ -213,7 +213,6 @@ if(WORLD_INTERFACE STREQUAL "REAL")
   add_library(can_interface STATIC)
   target_sources(can_interface PRIVATE
     CAN/CANBoard.cpp
-    CAN/CANMotor.cpp
     CAN/CANUtils.cpp
   )
 

src/world_interface/real_world_interface.cpp

@@ -288,6 +288,7 @@ callbackid_t addLimitSwitchCallback(
 	// auto nextID = nextCallbackID++;
 	// callbackIDMap.insert({nextID, id});
 	// return nextID;
+	return 0;
 }
 
 void removeLimitSwitchCallback(callbackid_t id) {