limitcalib test 1

huskyroboticsteam · Oct 24, 2023 · c732053 · c732053
1 parent 29ac65c
commit c732053
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 5 deletions.
diff --git a/src/LimitCalib.cpp b/src/LimitCalib.cpp
@@ -2,10 +2,14 @@
 #include "utils/threading.h"
 #include "world_interface/real_world_constants.h"
 #include "world_interface/world_interface.h"
+#include "control/TrapezoidalVelocityProfile.h"
 
 #include <chrono>
 #include <csignal>
+//#include <Eigen/core>
+#include <iostream>
 
+using namespace Eigen;
 using namespace robot;
 using namespace std::chrono;
 using robot::types::DataPoint;
@@ -53,4 +57,47 @@ int main() {
 	} while (!latch.wait_for(500ms));
 
 	log(LOG_INFO, "Done\n");
+
+	constexpr std::array<robot::types::motorid_t, 2> sortedKeys = {
+		{robot::types::motorid_t::shoulder, robot::types::motorid_t::elbow}
+	};
+
+	std::sort(sortedKeys.begin(), sortedKeys.end());
+
+	control::TrapezoidalVelocityProfile<encMotors.size()> profile(0.4, 0.6);
+	//std::vector<double, 2> targetPosVec;
+	navtypes::Vectord<2> targetPosVec; 
+	std::array<robot::types::motorid_t, 2> motorNames;
+
+	int i = 0;
+	for (const auto& runningMotor : encMotors) {
+		motorNames[i] = runningMotor.first;
+		targetPosVec[i] = runningMotor.second.stdPos;
+		i++;
+	}
+
+	// mess with putting the std Pos's in a vector to begin with 
+	// and then sort afterwards.
+	std::sort(targetPosVec.begin(), targetPosVec.end());
+
+	robot::types::datatime_t currTime(0s);
+
+	profile.setTarget(currTime, getMotorPositionsRad(motorNames), targetPosVec);
+
+	JacobianVelController<2,2> controller([](Eigen::VectorXd x) { return x; }, {});
+
+	bool done = false;
+	while(!done) {
+		navtypes::Vectord<2> targetVel = profile.getCommand(currTime);
+		double d;
+		navtypes::Vectord<2>  targetPos = controller.getCommand(currTime, targetVel, d);
+		i = 0;
+		for (const auto& element : sortedKeys) {
+			robot::setMotorPos(element, targetPos[i]);	
+			i++;
+		}
+		if (i == sortedKeys.size()) {
+			done = true;
+		}
+	}
 }
diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h
@@ -58,23 +58,26 @@ struct encparams_t {
 	int limitSwitchHigh;
 	/** Power value set during limit switch calibration */
 	double zeroCalibrationPower;
+	/** Standard position to return motors to after calibration */
+	double stdPos;
 };
 
 // clang-format off
-constexpr auto encMotors = frozen::make_unordered_map<motorid_t, encparams_t>({
-	{motorid_t::shoulder,
+constexpr auto encMotors = frozen::make_unordered_map<motorid_t, encparams_t>(
+	{{motorid_t::shoulder,
 		{.isInverted = true,
 		.ppjr = 4590 * 1024 * 4,
 		.limitSwitchLow = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::shoulder).first,
 		.limitSwitchHigh = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::shoulder).second,
-		.zeroCalibrationPower = 0.4}},
+		.zeroCalibrationPower = 0.4,
+		.stdPos = 2*(M_PI/3)}},
 	{motorid_t::elbow,
 		{.isInverted = false,
 		.ppjr = 1620 * 1024 * 4,
 		.limitSwitchLow = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::elbow).first,
 		.limitSwitchHigh = Constants::arm::JOINT_LIMITS.at(robot::types::motorid_t::elbow).second,
-		.zeroCalibrationPower = -0.15}}
-});
+		.zeroCalibrationPower = -0.15,
+		.stdPos = 0.5*(M_PI)}}});
 // clang-format on
 
 // TODO: find appropriate bounds