Servos & Steppers (#368)

* Add basic set requested motor power to _power_repeat_task

* Add motor power request handling

* Fix formatting

* Update loops

* Start adding device groups to CAN Motors

* Update getMotorPos

* Update the rest of the CAN Motor functions to use deviceid_t

* Update real world interface with device groups

* Fix stupid motor error

* Update HindsightCAN

* Add device groups

* Fix merge errors

* Revert HindsightCAN, remove unused global

* Fix formatting

* Fix formatting

* Update drill serials

* add quick fourbar (remove)

* PWM scale updates

* Remove motor power requests

* Add four bar linkage encoder, update four bar as joints

* Set encoder as potentiometer, fix linkage defs

* Fix merge, update fourbar pot configs

* Fix linkage get joint pos

* Fix formatting

* Update Serials

* Fix Syntax

* Servos

* Set servo board as a motor, add servo functions to interface

* Add PWM scale, error checking

* Remove extra init

* Add print packet, stepper fake can

* Update pring, step packet

* Fix servo request

* Add CAN Packet

* Add stepper control

---------

Co-authored-by: Varun <vvijayababu4334@gmail.com>

Author: imisaacwu

src/CAN/CAN.cpp

@@ -285,6 +285,20 @@ void sendCANPacket(const CANPacket& packet) {
 	}
 }
 
+void printCANPacket(const CANPacket& packet) {
+  std::stringstream ss;
+  ss << "CAN: p" << std::hex << ((packet.id >> 10) & 0x1);
+  ss << " g" << std::hex << ((packet.id & 0x03C0) >> 6);
+  ss << " s" << std::hex << ((packet.id & 0x003F));
+  ss << " pid" << std::hex << static_cast<uint>(packet.data[0]);
+  ss << " data:";
+  for (int i = 1; i < packet.dlc; i++) {
+    ss << std::hex << static_cast<uint>(packet.data[i]) << " ";
+  }
+
+  LOG_F(INFO, ss.str().c_str());
+}
+
 robot::types::DataPoint<telemetry_t> getDeviceTelemetry(deviceid_t id, telemtype_t telemType) {
 	std::shared_lock mapLock(telemMapMutex); // acquire read lock
 	// find entry for device in map

src/CAN/CAN.h

@@ -43,6 +43,9 @@ void initCAN();
  */
 void sendCANPacket(const CANPacket& packet);
 
+// Print packet for debugging purposes
+void printCANPacket(const CANPacket& packet);
+
 /**
  * @brief Get the latest telemetry from a CAN device.
  *

src/CAN/CANMotor.cpp

@@ -11,6 +11,7 @@ extern "C" {
 #include <HindsightCAN/CANCommon.h>
 #include <HindsightCAN/CANMotorUnit.h>
 #include <HindsightCAN/CANPacket.h>
+#include <HindsightCAN/CANScience.h>
 }
 
 using namespace std::chrono_literals;
@@ -121,10 +122,16 @@ void setMotorPIDTarget(devicegroup_t group, deviceserial_t serial, int32_t targe
 
 void setServoPos(devicegroup_t group, deviceserial_t serial, uint8_t servoNum, int32_t angle) {
 	CANPacket p;
-	AssemblePCAServoPacket(&p, static_cast<uint8_t>(group), serial, servoNum, angle);
+	AssembleScienceServoPacket(&p, static_cast<uint8_t>(group), serial, servoNum, angle);
 	sendCANPacket(p);
 }
 
+void setStepperTurnAngle(devicegroup_t group, deviceserial_t serial, uint8_t stepper, int16_t angle) {
+  CANPacket p;
+  AssembleScienceStepperTurnAnglePacket(&p, static_cast<uint8_t>(group), serial, stepper, angle, 0x3);
+  sendCANPacket(p);
+}
+
 DataPoint<int32_t> getMotorPosition(devicegroup_t group, deviceserial_t serial) {
 	return getDeviceTelemetry(std::make_pair(group, serial), telemtype_t::angle);
 }

src/CAN/CANMotor.h

@@ -148,10 +148,12 @@ void setMotorPIDTarget(devicegroup_t group, deviceserial_t serial, int32_t targe
  *
  * @param serial The CAN serial number of the motor board.
  * @param servoNum the servo number.
- * @param angle the angle of the servo in millidegrees.
+ * @param position the position of the servo in degrees.
  */
 void setServoPos(devicegroup_t group, deviceserial_t serial, uint8_t servoNum, int32_t angle);
 
+void setStepperTurnAngle(devicegroup_t group, deviceserial_t serial, uint8_t stepper, int16_t angle);
+
 /**
  * @brief Get the last reported position of a motor.
  *

src/CAN/FakeCANBoard.cpp

@@ -12,6 +12,7 @@
 #include <vector>
 
 extern "C" {
+#include <HindsightCAN/CANPower.h>
 #include <HindsightCAN/CANScience.h>
 }
 
@@ -28,14 +29,17 @@ enum class TestMode {
 	Encoder,
 	LimitSwitch,
 	Telemetry,
-	ScienceServos
+	ScienceServos,
+  Stepper,
+  RawCAN
 };
 
 std::unordered_set<int> modes = {
 	static_cast<int>(TestMode::ModeSet),   static_cast<int>(TestMode::PWM),
 	static_cast<int>(TestMode::PID),	   static_cast<int>(TestMode::Encoder),
 	static_cast<int>(TestMode::PIDVel),	   static_cast<int>(TestMode::LimitSwitch),
-	static_cast<int>(TestMode::Telemetry), static_cast<int>(TestMode::ScienceServos)};
+	static_cast<int>(TestMode::Telemetry), static_cast<int>(TestMode::ScienceServos),
+  static_cast<int>(TestMode::Stepper), static_cast<int>(TestMode::RawCAN)};
 
 int prompt(std::string_view message) {
 	std::string str;
@@ -59,8 +63,6 @@ int prompt(std::string_view message) {
 int main() {
 	can::initCAN();
 
-	CANPacket p;
-	uint8_t science_group = static_cast<int>(can::devicegroup_t::science);
 	std::stringstream ss("What are you testing?\n");
 	ss << static_cast<int>(TestMode::ModeSet) << " for MODE SET\n";
 	ss << static_cast<int>(TestMode::PWM) << " for PWM\n";
@@ -69,6 +71,8 @@ int main() {
 	ss << static_cast<int>(TestMode::LimitSwitch) << " for LIMIT SWITCH\n";
 	ss << static_cast<int>(TestMode::Telemetry) << " for TELEMETRY\n";
 	ss << static_cast<int>(TestMode::ScienceServos) << " for SCIENCE SERVOS\n";
+  ss << static_cast<int>(TestMode::Stepper) << " for STEPPER\n";
+  ss << static_cast<int>(TestMode::RawCAN) << " for CAN\n";
 	int test_type = prompt(ss.str().c_str());
 	if (modes.find(test_type) == modes.end()) {
 		std::cout << "Unrecognized response: " << test_type << std::endl;
@@ -287,10 +291,37 @@ int main() {
 		} else if (testMode == TestMode::ScienceServos) {
 			int servo_no = prompt("Enter servo no");
 			int degrees = prompt("Enter degrees");
-			// AssembleScienceServoPacket(&p, science_group, 0x0, (uint8_t)servo_no,
-			// 						   (uint8_t)degrees);
-			AssembleScienceStepperTurnAnglePacket(&p, science_group, 0x3, (uint8_t) servo_no, degrees, 0x3);
+
+			CANPacket p;
+			AssembleScienceServoPacket(&p, 0x7, 0x5, (uint8_t)servo_no,
+									   (uint8_t)degrees);
 			can::sendCANPacket(p);
-		}
+      can::printCANPacket(p);
+		} else if (testMode == TestMode::Stepper) {
+      int stepper = prompt("Enter stepper");
+      int angle = prompt("Enter angle");
+
+      CANPacket p;
+      AssembleScienceStepperTurnAnglePacket(&p, 0x7, 0x4, stepper, angle, 0x3);
+      can::sendCANPacket(p);
+      can::printCANPacket(p);
+    } else if (testMode == TestMode::RawCAN) {
+      uint8_t pr = prompt("priority");
+      uint8_t g = prompt("group");
+      uint8_t s = prompt("serial");
+      uint8_t pid = prompt("pid");
+      uint8_t dlc = prompt("add'l. data bits");
+      uint8_t data[dlc+1];
+      data[0] = pid;
+
+      for (int i = 1; i <= dlc; i++) {
+        data[i] = prompt("bit");
+      }
+
+      CANPacket p;
+      AssembleCANPacket(&p, pr, g, s, pid, dlc+1, data);
+      can::sendCANPacket(p);
+      can::printCANPacket(p);
+    }
 	}
 }

src/CMakeLists.txt

@@ -94,7 +94,7 @@ find_package(nlohmann_json 3.2.0 REQUIRED)
 FetchContent_Declare(
     HindsightCAN
     GIT_REPOSITORY https://github.com/huskyroboticsteam/HindsightCAN.git
-    GIT_TAG 6ae5db46856cd1f745ad5e1745a6983f4fc372c1
+    GIT_TAG ce04c1a2dced3eb0e5beaaf39b2fcc436655e790
 )
 FetchContent_MakeAvailable(HindsightCAN)
 

src/control_interface.h

@@ -50,4 +50,5 @@ void setJointPos(types::jointid_t joint, int32_t targetPos);
  */
 types::DataPoint<int32_t> getJointPos(types::jointid_t joint);
 
+void setScienceServoPos(uint8_t servoNum, int32_t angle);
 } // namespace robot

src/network/MissionControlMessages.h

@@ -3,6 +3,7 @@
 namespace net::mc {
 
 // TODO: possibly use frozen::string for this so we don't have to use raw char ptrs
+
 // request keys
 constexpr const char* EMERGENCY_STOP_REQ_TYPE = "emergencyStopRequest";
 constexpr const char* OPERATION_MODE_REQ_TYPE = "operationModeRequest";
@@ -15,6 +16,8 @@ constexpr const char* CAMERA_STREAM_OPEN_REQ_TYPE = "cameraStreamOpenRequest";
 constexpr const char* CAMERA_STREAM_CLOSE_REQ_TYPE = "cameraStreamCloseRequest";
 constexpr const char* CAMERA_FRAME_REQ_TYPE = "cameraFrameRequest";
 constexpr const char* WAYPOINT_NAV_REQ_TYPE = "waypointNavRequest";
+constexpr const char* SERVO_POSITION_REQ_TYPE = "servoPositionRequest";
+constexpr const char* STEPPER_TURN_ANGLE_REQ_TYPE = "stepperTurnAngleRequest";
 
 // report keys
 constexpr const char* CAMERA_STREAM_REP_TYPE = "cameraStreamReport";
@@ -23,5 +26,6 @@ constexpr const char* MOUNTED_PERIPHERAL_REP_TYPE = "mountedPeripheralReport";
 constexpr const char* JOINT_POSITION_REP_TYPE = "jointPositionReport";
 constexpr const char* ROVER_POS_REP_TYPE = "roverPositionReport";
 constexpr const char* ARM_IK_ENABLED_REP_TYPE = "armIKEnabledReport";
+constexpr const char* SERVO_POSITION_REP_TYPE = "servoPositionReport";
 
 } // namespace net::mc

src/network/MissionControlProtocol.cpp

@@ -176,6 +176,52 @@ void MissionControlProtocol::handleJointPowerRequest(const json& j) {
 	}
 }
 
+static bool validateJointPositionRequest(const json& j) {
+	return validateJoint(j) && util::validateKey(j, "position", val_t::number_integer);
+}
+
+static void handleJointPositionRequest([[maybe_unused]] const json& j) {
+	// TODO: ignore this message if we are in autonomous mode.
+	// std::string motor = j["joint"];
+	// double position_deg = j["position"];
+	// int32_t position_mdeg = std::round(position_deg * 1000);
+	// TODO: actually implement joint position requests
+	// setMotorPos(motor, position_mdeg);
+}
+
+static bool validateServo(const json& j) {
+	return util::validateKey(j, "servo", val_t::string);/* &&
+		   std::any_of(all_jointid_t.begin(), all_jointid_t.end(), [&](const auto& joint) {
+			   return j["joint"].get<std::string>() == util::to_string(joint);
+		   });*/
+}
+
+static bool validateServoPositionRequest(const json& j) {
+  return validateServo(j);// && util::validateKey(j, "position", val_t::number_integer);
+}
+
+void MissionControlProtocol::handleServoPositionRequest(const json& j) {
+  std::string servoName = j["servo"];
+  int32_t position = j["position"];
+  auto servo = name_to_servoid.find(util::freezeStr(servoName));
+  if (servo != name_to_servoid.end()) {
+	  robot::setServoPos(servo->second, position);
+  }
+}
+
+static bool validateStepperTurnAngleRequest(const json& j) {
+  return util::hasKey(j, "stepper");
+}
+
+void MissionControlProtocol::handleStepperTurnAngleRequest(const json& j) {
+  std::string stepperName = j["stepper"];
+  int16_t angle = j["angle"];
+  auto stepper = name_to_stepperid.find(util::freezeStr(stepperName));
+  if (stepper != name_to_stepperid.end()) {
+    robot::setRequestedStepperTurnAngle(stepper->second, angle);
+  }
+}
+
 static bool validateWaypointNavRequest(const json& j) {
 	bool lat_is_unsigned = util::validateKey(j, "latitude", val_t::number_unsigned);
 	bool lon_is_unsigned = util::validateKey(j, "longitude", val_t::number_unsigned);
@@ -202,19 +248,6 @@ void MissionControlProtocol::handleWaypointNavRequest(const json& j) {
 	}
 }
 
-static bool validateJointPositionRequest(const json& j) {
-	return validateJoint(j) && util::validateKey(j, "position", val_t::number_integer);
-}
-
-static void handleJointPositionRequest([[maybe_unused]] const json& j) {
-	// TODO: ignore this message if we are in autonomous mode.
-	// std::string motor = j["joint"];
-	// double position_deg = j["position"];
-	// int32_t position_mdeg = std::round(position_deg * 1000);
-	// TODO: actually implement joint position requests
-	// setMotorPos(motor, position_mdeg);
-}
-
 static bool validateCameraStreamOpenRequest(const json& j) {
 	// return util::validateKey(j, "camera", val_t::number_integer);
 	return util::hasKey(j, "camera");
@@ -352,6 +385,14 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 		WAYPOINT_NAV_REQ_TYPE,
 		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1),
 		validateWaypointNavRequest);
+  this->addMessageHandler(
+    SERVO_POSITION_REQ_TYPE,
+    std::bind(&MissionControlProtocol::handleServoPositionRequest, this, _1),
+    validateServoPositionRequest);
+  this->addMessageHandler(
+    STEPPER_TURN_ANGLE_REQ_TYPE,
+    std::bind(&MissionControlProtocol::handleStepperTurnAngleRequest, this, _1),
+    validateStepperTurnAngleRequest);
 	this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this));
 	this->addDisconnectionHandler(
 		std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this, false));

src/network/MissionControlProtocol.h

@@ -51,6 +51,8 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 	void handleWaypointNavRequest(const json& j);
 	void handleDriveRequest(const json& j);
 	void handleRequestArmIKEnabled(const json& j);
+  void handleServoPositionRequest(const json& j);
+  void handleStepperTurnAngleRequest(const json& j);
 	void sendArmIKEnabledReport(bool enabled);
 	void handleConnection();
 	void handleHeartbeatTimedOut();