Merge branch 'master' into inverse-kinematics-requests

rover-config/50-rover-cameras.rules

@@ -7,10 +7,10 @@
 # Install this in /etc/udev/rules.d
 
 # Mast camera
-SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.3", SYMLINK+="video20"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.2", SYMLINK+="video20"
 
 # Forearm camera
-SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.1", SYMLINK+="video30"
+# SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.1", SYMLINK+="video30"
 
 # Hand camera
-SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", SYMLINK+="video40"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.1", SYMLINK+="video40"

rover-config/rover-config.sh

@@ -1,7 +1,5 @@
 #!/usr/bin/env bash
-cp -r ./.motion/ /home/$USER
 sudo cp ./50-rover-cameras.rules /etc/udev/rules.d/
-sudo cp ./50-usb-hokuyo-lidar.rules /etc/udev/rules.d/
 
 # Reloads udev rules instead of having to reboot
 sudo udevadm control --reload-rules

src/CAN/CAN.cpp

@@ -70,7 +70,8 @@ const std::unordered_map<telemetrycode_t, telemtype_t> telemCodeToTypeMap = {
 	{PACKET_TELEMETRY_GYRO_X, telemtype_t::gyro_x},
 	{PACKET_TELEMETRY_GYRO_Y, telemtype_t::gyro_y},
 	{PACKET_TELEMETRY_GYRO_Z, telemtype_t::gyro_z},
-	{PACKET_TELEMETRY_LIM_SW_STATE, telemtype_t::limit_switch}};
+	{PACKET_TELEMETRY_LIM_SW_STATE, telemtype_t::limit_switch},
+	{PACKET_TELEMETRY_ADC_RAW, telemtype_t::adc_raw}};
 
 // the telemetry map will store telemetry code instead of telem enum
 // this means unrecognized telemetry types won't cause UB

src/CAN/CANUtils.h

@@ -48,8 +48,9 @@ enum class telemtype_t {
 	gyro_x = PACKET_TELEMETRY_GYRO_X,
 	gyro_y = PACKET_TELEMETRY_GYRO_Y,
 	gyro_z = PACKET_TELEMETRY_GYRO_Z,
-	limit_switch = PACKET_TELEMETRY_LIM_SW_STATE
-	// TODO: add further telemetry types if required
+	limit_switch = PACKET_TELEMETRY_LIM_SW_STATE,
+	adc_raw = PACKET_TELEMETRY_ADC_RAW
+	// add further telemetry types if required
 };
 
 /**

src/CAN/FakeCANBoard.cpp

@@ -27,20 +27,17 @@ enum class TestMode {
 	PIDVel,
 	Encoder,
 	LimitSwitch,
-	ScienceTelemetry,
+	Telemetry,
 	ScienceMotors,
 	ScienceServos
 };
 
-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::ScienceTelemetry),
-								 static_cast<int>(TestMode::ScienceMotors),
-								 static_cast<int>(TestMode::ScienceServos)};
+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::ScienceMotors),
+	static_cast<int>(TestMode::ScienceServos)};
 
 int prompt(std::string_view message) {
 	std::string str;
@@ -72,37 +69,32 @@ int main() {
 	ss << static_cast<int>(TestMode::PID) << " for PID\n";
 	ss << static_cast<int>(TestMode::Encoder) << " for ENCODER\n";
 	ss << static_cast<int>(TestMode::LimitSwitch) << " for LIMIT SWITCH\n";
-	ss << static_cast<int>(TestMode::ScienceTelemetry) << " for SCIENCE TELEMETRY\n";
+	ss << static_cast<int>(TestMode::Telemetry) << " for TELEMETRY\n";
 	ss << static_cast<int>(TestMode::ScienceMotors) << " for SCIENCE MOTORS\n";
-	ss << static_cast<int>(TestMode::ScienceTelemetry) << " for SCIENCE SERVOS\n";
+	ss << static_cast<int>(TestMode::ScienceServos) << " for SCIENCE SERVOS\n";
 	int test_type = prompt(ss.str().c_str());
 	if (modes.find(test_type) == modes.end()) {
 		std::cout << "Unrecognized response: " << test_type << std::endl;
 		std::exit(1);
 	}
 	TestMode testMode = static_cast<TestMode>(test_type);
-	int serial = 0;
-	if (testMode == TestMode::PWM || testMode == TestMode::PID ||
-		testMode == TestMode::Encoder || testMode == TestMode::LimitSwitch) {
-		serial = prompt("Enter motor serial");
-	}
 	bool mode_has_been_set = false;
 
 	while (true) {
 		if (testMode == TestMode::ModeSet) {
-			serial = prompt("Enter motor serial");
+			int serial = prompt("Enter motor serial");
 			int mode = prompt("Enter mode (0 for PWM, 1 for PID)");
 			std::cout << "got " << serial << " and " << mode << std::endl;
 			can::motor::setMotorMode(serial, mode == 0 ? motormode_t::pwm : motormode_t::pid);
 		} else if (testMode == TestMode::PWM) {
+			int serial = prompt("Enter motor serial");
 			int pwm = prompt("Enter PWM");
 			can::motor::setMotorMode(serial, motormode_t::pwm);
 			can::motor::setMotorPower(serial, static_cast<int16_t>(pwm));
 		} else if (testMode == TestMode::PID) {
-			// Don't send all five packets at once. On some motor boards, the CAN buffer
-			// only fits four packets.
-
+			static int serial;
 			if (!mode_has_been_set) {
+				serial = prompt("Enter motor serial");
 				// AVR board firmware resets the angle target every time it receives a
 				// mode set packet, so we only want to send this once.
 				// TODO: do we need to set the PPJR?
@@ -126,6 +118,8 @@ int main() {
 			static double vel_timeout;
 
 			if (!mode_has_been_set) {
+				int serial = prompt("Enter motor serial");
+
 				// set pid mode
 				can::motor::setMotorMode(serial, motormode_t::pid);
 				mode_has_been_set = true;
@@ -179,7 +173,10 @@ int main() {
 				motor->setMotorPower(0.0);
 			}
 		} else if (testMode == TestMode::Encoder) {
+			static int serial;
 			if (!mode_has_been_set) {
+				serial = prompt("Enter motor serial");
+
 				int sensorType;
 				do {
 					sensorType =
@@ -212,7 +209,9 @@ int main() {
 			std::this_thread::sleep_for(20ms);
 		} else if (testMode == TestMode::LimitSwitch) {
 			static bool testLimits = false;
+			static int serial;
 			if (!mode_has_been_set) {
+				serial = prompt("Enter motor serial");
 				testLimits = static_cast<bool>(prompt("Set limits? 1=yes,0=no"));
 				if (testLimits) {
 					int lo = prompt("Low position");
@@ -242,28 +241,39 @@ int main() {
 				std::cout << "\33[2K\rEncoder value: " << encoderStr << std::flush;
 				std::this_thread::sleep_for(20ms);
 			}
-		} else if (testMode == TestMode::ScienceTelemetry) {
+		} else if (testMode == TestMode::Telemetry) {
 			if (!mode_has_been_set) {
-				serial = 0x01;
-				can::deviceid_t id = std::make_pair(can::devicegroup_t::science, serial);
+				auto group = static_cast<can::devicegroup_t>(prompt("Enter group code"));
+				int serial = prompt("Enter serial");
+				can::deviceid_t deviceID = std::make_pair(group, serial);
+				auto telemType = static_cast<can::telemtype_t>(prompt("Enter telemetry type"));
 				can::addDeviceTelemetryCallback(
-					id, can::telemtype_t::limit_switch,
+					deviceID, telemType,
 					[](can::deviceid_t id, can::telemtype_t telemType,
 					   DataPoint<can::telemetry_t> data) {
-						std::cout << "Science: serial=" << std::hex
+						std::cout << "Telemetry: group=" << std::hex
+								  << static_cast<int>(id.first) << ", serial=" << std::hex
 								  << static_cast<int>(id.second) << ", type=" << std::hex
 								  << static_cast<int>(telemType) << ", data=" << std::dec
 								  << data.getDataOrElse(0) << std::endl;
 					});
+				int telemPeriod = prompt("Telemetry timing (ms)");
+				bool useTimingPacket =
+					static_cast<bool>(prompt("What telemetry method?\n0 for pull packets\n1 "
+											 "for telemetry timing packet"));
+				if (useTimingPacket) {
+					CANPacket packet;
+					AssembleTelemetryTimingPacket(
+						&packet, static_cast<uint8_t>(deviceID.first), deviceID.second,
+						static_cast<uint8_t>(telemType), telemPeriod);
+					can::sendCANPacket(packet);
+				} else {
+					can::scheduleTelemetryPull(deviceID, telemType,
+											   std::chrono::milliseconds(telemPeriod));
+				}
 				mode_has_been_set = true;
 			}
-			auto reqType = prompt("Enter telemetry type");
-			int telemPeriod = prompt("Telemetry timing (ms)");
-			CANPacket packet;
-			AssembleTelemetryTimingPacket(&packet,
-										  static_cast<uint8_t>(can::devicegroup_t::science),
-										  serial, reqType, telemPeriod);
-			can::sendCANPacket(packet);
+			std::this_thread::sleep_for(1s);
 		} else if (testMode == TestMode::ScienceMotors) {
 			// CAREFUL, there are no safety checks / limit switches
 			// 0: drill up/down (down is positive, ~500 PWM)

src/CMakeLists.txt

@@ -109,7 +109,7 @@ find_package(nlohmann_json 3.2.0 REQUIRED)
 
 # Find the CAN library; contains packet and protocol definitions and does not
 # actually require physical CAN to be present.
-find_package(HindsightCAN 1.3.2 REQUIRED)
+find_package(HindsightCAN 1.4.0 REQUIRED)
 
 find_package(Threads REQUIRED)
 find_package(OpenCV REQUIRED)

src/Constants.h

@@ -150,7 +150,7 @@ namespace arm {
 /**
  * Maximum commanded end-effector velocity, in m/s
  */
-constexpr double MAX_EE_VEL = 0.3;
+constexpr double MAX_EE_VEL = 0.1;
 constexpr double IK_SOLVER_THRESH = 0.001;
 
 constexpr int IK_SOLVER_MAX_ITER = 50;

src/network/MissionControlProtocol.cpp

@@ -134,15 +134,13 @@ void MissionControlProtocol::handleSetArmIKEnabled(const json& j) {
 		if (_arm_ik_repeat_thread.joinable()) {
 			_arm_ik_repeat_thread.join();
 		}
-		navtypes::Vectord<Constants::arm::IK_MOTORS.size()> armJointPositions;
-		for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {
-			auto positionDataPoint = robot::getMotorPos(Constants::arm::IK_MOTORS[i]);
-			assert(positionDataPoint.isValid()); // crash if data is not valid
-			double position = static_cast<double>(positionDataPoint.getData());
-			position *= M_PI / 180.0 / 1000.0;
-			armJointPositions(i) = position;
-		}
-		Globals::planarArmController.set_setpoint(armJointPositions);
+		DataPoint<navtypes::Vectord<Constants::arm::IK_MOTORS.size()>> armJointPositions =
+			robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
+		// TODO: there should be a better way of handling invalid data than crashing.
+		// It should somehow just not enable IK, but then it needs to communicate back to MC
+		// that IK wasn't enabled?
+		assert(armJointPositions.isValid());
+		Globals::planarArmController.set_setpoint(armJointPositions.getData());
 		Globals::armIKEnabled = true;
 		_arm_ik_repeat_thread =
 			std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this);
@@ -164,9 +162,9 @@ void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) {
 
 static bool validateJoint(const json& j) {
 	return util::validateKey(j, "joint", val_t::string) &&
-		   util::validateOneOf(j, "joint",
-							   {"armBase", "shoulder", "elbow", "forearm", "wrist", "hand",
-								"ikUp", "ikForward"});
+		   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 validateJointPowerRequest(const json& j) {
@@ -430,21 +428,17 @@ void MissionControlProtocol::jointPowerRepeatTask() {
 void MissionControlProtocol::updateArmIKRepeatTask() {
 	dataclock::time_point next_update_time = dataclock::now();
 	while (Globals::armIKEnabled) {
-		navtypes::Vectord<Constants::arm::IK_MOTORS.size()> armJointPositions;
-		for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {
-			DataPoint<int32_t> positionDataPoint =
-				robot::getMotorPos(Constants::arm::IK_MOTORS[i]);
-			assert(positionDataPoint.isValid()); // crash if data is not valid
-			double position = static_cast<double>(positionDataPoint.getData());
-			position *= M_PI / 180.0 / 1000.0;
-			armJointPositions(i) = position;
-		}
-		navtypes::Vectord<Constants::arm::IK_MOTORS.size()> targetJointPositions =
-			Globals::planarArmController.getCommand(dataclock::now(), armJointPositions);
-		targetJointPositions /= M_PI / 180.0 / 1000.0; // convert from radians to millidegrees
-		for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {
-			robot::setMotorPos(Constants::arm::IK_MOTORS[i],
-							   static_cast<int32_t>(targetJointPositions(i)));
+		DataPoint<navtypes::Vectord<Constants::arm::IK_MOTORS.size()>> armJointPositions =
+			robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
+		if (armJointPositions.isValid()) {
+			navtypes::Vectord<Constants::arm::IK_MOTORS.size()> targetJointPositions =
+				Globals::planarArmController.getCommand(dataclock::now(), armJointPositions);
+			targetJointPositions /=
+				M_PI / 180.0 / 1000.0; // convert from radians to millidegrees
+			for (size_t i = 0; i < Constants::arm::IK_MOTORS.size(); i++) {
+				robot::setMotorPos(Constants::arm::IK_MOTORS[i],
+								   static_cast<int32_t>(targetJointPositions(i)));
+			}
 		}
 		next_update_time += Constants::ARM_IK_UPDATE_PERIOD;
 		std::this_thread::sleep_until(next_update_time);

src/world_interface/data.cpp

@@ -42,8 +42,6 @@ std::string to_string(robot::types::jointid_t joint) {
 			return "hand";
 		case jointid_t::wrist:
 			return "wrist";
-		case jointid_t::drill_arm:
-			return "drillArm";
 		case jointid_t::activeSuspension:
 			return "activeSuspension";
 		case jointid_t::ikUp:

src/world_interface/data.h

@@ -82,16 +82,15 @@ enum class jointid_t {
 	forearm,
 	wrist,
 	hand,
-	drill_arm,
 	activeSuspension,
 	ikForward,
 	ikUp
 };
 
 constexpr auto all_jointid_t = frozen::make_unordered_set<jointid_t>(
 	{jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm,
-	 jointid_t::wrist, jointid_t::hand, jointid_t::drill_arm, jointid_t::activeSuspension,
-	 jointid_t::ikForward, jointid_t::ikUp});
+	 jointid_t::wrist, jointid_t::hand, jointid_t::activeSuspension, jointid_t::ikForward,
+	 jointid_t::ikUp});
 
 constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
 	{{"armBase", jointid_t::armBase},
@@ -100,7 +99,6 @@ constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, join
 	 {"forearm", jointid_t::forearm},
 	 {"wrist", jointid_t::wrist},
 	 {"hand", jointid_t::hand},
-	 {"drillArm", jointid_t::drill_arm},
 	 {"activeSuspension", jointid_t::activeSuspension},
 	 {"ikForward", jointid_t::ikForward},
 	 {"ikUp", jointid_t::ikUp}});

src/world_interface/kinematic_common_interface.cpp

@@ -125,16 +125,25 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) {
 			util::to_string(joint).c_str());
 	}
 }
+
 types::DataPoint<int32_t> getJointPos(robot::types::jointid_t joint) {
 	if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
 		return getMotorPos(Constants::JOINT_MOTOR_MAP.at(joint));
-	}
-	// FIXME: need to do some extra work for differential - we will have to figure out which
-	// motor boards the potentiometers are plugged into and query those for "motor position"
-	else {
-		// FIXME: this should ideally never happen, but we don't have support for all joints
-		// yet because we don't know anything about the drill arm (and need to do extra work
-		// for the differential)
+	} else if (joint == jointid_t::ikForward || joint == jointid_t::ikUp) {
+		DataPoint<navtypes::Vectord<Constants::arm::IK_MOTORS.size()>> armJointPositions =
+			robot::getMotorPositionsRad(Constants::arm::IK_MOTORS);
+		if (armJointPositions.isValid()) {
+			Eigen::Vector2d eePos =
+				Globals::planarArmKinematics.jointPosToEEPos(armJointPositions.getData());
+			Eigen::Vector2i eePosInt = (1000 * eePos).array().round().cast<int>();
+			return DataPoint<int32_t>(armJointPositions.getTime(),
+									  joint == jointid_t::ikForward ? eePosInt.x()
+																	: eePosInt.y());
+		} else {
+			return {};
+		}
+	} else {
+		// This should ideally never happen, but may if we haven't implemented a joint yet.
 		log(LOG_WARN, "getJointPos called for currently unsupported joint %s\n",
 			util::to_string(joint).c_str());
 		return {};

src/world_interface/real_world_constants.h

@@ -82,11 +82,11 @@ constexpr auto potMotors = frozen::make_unordered_map<motorid_t, potparams_t>(
 	{{motorid_t::armBase,
 	  {.adc_lo = 123, .mdeg_lo = -200 * 1000, .adc_hi = 456, .mdeg_hi = 200 * 1000}},
 	 {motorid_t::forearm,
-	  {.adc_lo = 123, .mdeg_lo = -360 * 1000, .adc_hi = 456, .mdeg_hi = 360 * 1000}},
+	  {.adc_lo = 1208, .mdeg_lo = -180 * 1000, .adc_hi = 841, .mdeg_hi = 180 * 1000}},
 	 {motorid_t::wrist,
 	  {.adc_lo = 123, .mdeg_lo = -100 * 1000, .adc_hi = 456, .mdeg_hi = 100 * 1000}},
 	 {motorid_t::activeSuspension,
-	  {.adc_lo = 123, .mdeg_lo = -100 * 1000, .adc_hi = 456, .mdeg_hi = 100 * 1000}}});
+	  {.adc_lo = 251, .mdeg_lo = -19 * 1000, .adc_hi = 1645, .mdeg_hi = 31 * 1000}}});
 
 /** @brief A mapping of motorids to their corresponding serial number. */
 constexpr auto motorSerialIDMap = frozen::make_unordered_map<motorid_t, can::deviceserial_t>(
@@ -121,7 +121,7 @@ constexpr auto positive_pwm_scales =
 												   {motorid_t::rearLeftWheel, 0.7},
 												   {motorid_t::rearRightWheel, 0.7},
 												   {motorid_t::hand, -0.75},
-												   {motorid_t::activeSuspension, 1.0}});
+												   {motorid_t::activeSuspension, -0.5}});
 /**
  * @brief A mapping of motorids to power scale factors when commanded with negative power.
  * Negative values mean that the motor is inverted.
@@ -137,6 +137,6 @@ constexpr auto negative_pwm_scales =
 												   {motorid_t::rearLeftWheel, 0.7},
 												   {motorid_t::rearRightWheel, 0.7},
 												   {motorid_t::hand, -0.75},
-												   {motorid_t::activeSuspension, 1.0}});
+												   {motorid_t::activeSuspension, -0.5}});
 
 } // namespace robot

src/world_interface/real_world_interface.cpp

@@ -129,7 +129,7 @@ void openCamera(CameraID camID, const char* cameraPath) {
 }
 
 void setupCameras() {
-	// openCamera(Constants::AR_CAMERA_ID, Constants::AR_CAMERA_CONFIG_PATH);
+	openCamera(Constants::AR_CAMERA_ID, Constants::AR_CAMERA_CONFIG_PATH);
 	openCamera(Constants::HAND_CAMERA_ID, Constants::HAND_CAMERA_CONFIG_PATH);
 }
 } // namespace