feat: add j3,j4,j5

MotionCore/Arm.cpp

@@ -31,12 +31,45 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 		oSerialBus,
 		-Vector3d::X,
 		Vector3d::Up,
-		95.0, //mm
+		97.0, //mm
-		2,
+		2, // index
-		-123,
+		-90, // min
-		57.0,
+		90.0, // max
-		0.0,
+		0.0, // home
 		&j1
+	),
+	j3(
+		oSerialBus,
+		Vector3d::X,
+		Vector3d::Up,
+		114.0, //mm
+		3,
+		-90.0,
+		90.0,
+		90.0,
+		&j2
+	),
+	j4(
+		oSerialBus,
+		Vector3d::Y,
+		Vector3d::Forward,
+		0.0, //mm
+		4,
+		-90.0,
+		90.0,
+		90.0,
+		&j3
+	),
+	j5(
+		oSerialBus,
+		-Vector3d::X,
+		Vector3d::Up,
+		56.0, //mm
+		5,
+		-90.0,
+		90.0,
+		90.0,
+		&j4
 	)
 {
 	if (!oSerialBus.isOpen()) {
@@ -56,15 +89,34 @@ bool Arm::assumeHomePose()
 	if (!j2.goHome(0.5)) {
 		return false;
 	}
+	if (!j3.goHome(0.5)) {
+		return false;
+	}
+	if (!j4.goHome(0.5)) {
+		return false;
+	}
+	if (!j5.goHome(0.5)) {
+		return false;
+	}
 
 	return true;
 }
 
-void Arm::stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept
+void Arm::stagePose(
+	const double theta_j0,
+	const double theta_j1,
+	const double theta_j2,
+	const double theta_j3,
+	const double theta_j4,
+	const double theta_j5
+) noexcept
 {
 	j0.stageAngle(theta_j0);
 	j1.stageAngle(theta_j1);
 	j2.stageAngle(theta_j2);
+	j3.stageAngle(theta_j3);
+	j4.stageAngle(theta_j4);
+	j5.stageAngle(theta_j5);
 }
 
 void Arm::commitPose() noexcept
@@ -78,6 +130,15 @@ void Arm::commitPose() noexcept
 	if (j2.isStaged()) {
 		j2.commitStaging();
 	}
+	if (j3.isStaged()) {
+		j3.commitStaging();
+	}
+	if (j4.isStaged()) {
+		j4.commitStaging();
+	}
+	if (j5.isStaged()) {
+		j5.commitStaging();
+	}
 }
 
 void Arm::rollbackPose() noexcept
@@ -91,19 +152,28 @@ void Arm::rollbackPose() noexcept
 	if (j2.isStaged()) {
 		j2.rollbackStaging();
 	}
+	if (j3.isStaged()) {
+		j3.rollbackStaging();
+	}
+	if (j4.isStaged()) {
+		j4.rollbackStaging();
+	}
+	if (j5.isStaged()) {
+		j5.rollbackStaging();
+	}
 }
 
 bool Arm::isMoving() const noexcept
 {
-	return j0.isMoving() || j1.isMoving() || j2.isMoving();
+	return j0.isMoving() || j1.isMoving() || j2.isMoving() || j3.isMoving() || j4.isMoving() || j5.isMoving();
 }
 
 bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept
 {
 	// Otherwise: check for endeffector position
-	const Vector3d ef = j2.getTargetedGlobalEndpoint();
+	const Vector3d ef = j5.getTargetedGlobalEndpoint();
 
-	// If any deadzone intersects with the EF pos, fail
+	// If any deadzone intersects with the EF pos, fai
 	for (const AxisAlignedBoundingBox& aabb : deadzones) {
 		if (aabb.doesIntersect(ef)) {
 			return false;
@@ -120,14 +190,26 @@ bool Arm::rollbackJointsBasedOnValidRange() noexcept
 		j0.rollbackStaging();
 		allGood = false;
 	}
-	if (!j1.isPositionInRange(j0.getTargetPosition())) {
+	if (!j1.isPositionInRange(j1.getTargetPosition())) {
 		j2.rollbackStaging();
 		allGood = false;
 	}
-	if (!j2.isPositionInRange(j0.getTargetPosition())) {
+	if (!j2.isPositionInRange(j2.getTargetPosition())) {
 		j2.rollbackStaging();
 		allGood = false;
 	}
+	if (!j3.isPositionInRange(j3.getTargetPosition())) {
+		j3.rollbackStaging();
+		allGood = false;
+	}
+	if (!j4.isPositionInRange(j4.getTargetPosition())) {
+		j4.rollbackStaging();
+		allGood = false;
+	}
+	if (!j5.isPositionInRange(j5.getTargetPosition())) {
+		j5.rollbackStaging();
+		allGood = false;
+	}
 	return allGood;
 }
 
@@ -147,6 +229,15 @@ bool Arm::assumeHomePoseImmediate()
 	if (!j2.goHomeImmediate()) {
 		return false;
 	}
+	if (!j3.goHomeImmediate()) {
+		return false;
+	}
+	if (!j4.goHomeImmediate()) {
+		return false;
+	}
+	if (!j5.goHomeImmediate()) {
+		return false;
+	}
 
 	return true;
 }
@@ -156,4 +247,7 @@ void Arm::update(double frametime)
 	j0.update(frametime);
 	j1.update(frametime);
 	j2.update(frametime);
+	j3.update(frametime);
+	j4.update(frametime);
+	j5.update(frametime);
 }

MotionCore/Arm.h

@@ -15,7 +15,14 @@ public:
 	bool assumeHomePoseImmediate();
 	bool assumeHomePose();
 
-	void stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept;
+	void stagePose(
+		const double theta_j0,
+		const double theta_j1,
+		const double theta_j2,
+		const double theta_j3,
+		const double theta_j4,
+		const double theta_j5
+	) noexcept;
 	void commitPose() noexcept;
 	void rollbackPose() noexcept;
 	bool isMoving() const noexcept;
@@ -28,6 +35,9 @@ public:
 	const ArmSegment& getJ0() const { return j0; };
 	const ArmSegment& getJ1() const { return j1; };
 	const ArmSegment& getJ2() const { return j2; };
+	const ArmSegment& getJ3() const { return j3; };
+	const ArmSegment& getJ4() const { return j4; };
+	const ArmSegment& getJ5() const { return j5; };
 
 	void update(double frametime);
 
@@ -36,5 +46,8 @@ private:
 	ArmSegment j0;
 	ArmSegment j1;
 	ArmSegment j2;
+	ArmSegment j3;
+	ArmSegment j4;
+	ArmSegment j5;
 };
 

MotionCore/ArmSegment.cpp

@@ -149,7 +149,7 @@ Vector3d ArmSegment::getCurrentGlobalEndpoint() const
 {
 	// Collect the chain from this segment up to the root
 	std::vector<const ArmSegment*> chain;
-	chain.reserve(3);
+	chain.reserve(4);
 	const ArmSegment* s = this;
 	while (s != nullptr) {
 		chain.push_back(s);

MotionCore/MainLoop.cpp

@@ -13,6 +13,10 @@ MainLoop::MainLoop() noexcept :
         AxisAlignedBoundingBox(
 			Vector3d(-125, -125, -20),
 			Vector3d(65, 100, 120)
+        ),
+        AxisAlignedBoundingBox(
+			Vector3d(0, -160, 0),
+			Vector3d(20000, 100, 20000)
         )
     }),
     leftVKB(L" VKBsim Gladiator EVO  L  ")
@@ -50,11 +54,14 @@ void MainLoop::run()
     static std::chrono::steady_clock::time_point lastBegin =
         std::chrono::high_resolution_clock::now();
 
-    //YZConsolePlotter plot(-200, 200, 20, 2);
+    YZConsolePlotter plot(-200, 200, 20, 2);
 
     double j0Pos = 0;
     double j1Pos = 0;
     double j2Pos = 0;
+    double j3Pos = 0;
+    double j4Pos = 0;
+    double j5Pos = 0;
 
     while (isRunning)
     {
@@ -79,28 +86,35 @@ void MainLoop::run()
                 j0Pos += -vkbi.roll * speed * frametime;
                 j1Pos += -vkbi.pitch * speed * frametime;
                 j2Pos += -vkbi.yaw * speed * frametime;
+                j3Pos = vkbi.wheel * (arm.getJ3().getMaxAngle() - arm.getJ3().getMinAngle());
+                j4Pos += vkbi.thumb_ver * speed * frametime;
+                j5Pos += vkbi.thumb_hor * speed * frametime;
 
                 // Stage the new pose and keep it if it is good
-                arm.stagePose(j0Pos, j1Pos, j2Pos);
+                arm.stagePose(j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos);
                 if (arm.isEFPositionValid(deadzones)) {
                     // Roll back individual joints if their angles are out of range
                     arm.rollbackJointsBasedOnValidRange();
 
-                    std::cout << "Pose good!!" << std::endl;
+                    //std::cout << "Pose good!!" << std::endl;
                     arm.commitPose();
 
                     j0Pos = arm.getJ0().getTargetPosition();
                     j1Pos = arm.getJ1().getTargetPosition();
                     j2Pos = arm.getJ2().getTargetPosition();
+                    j3Pos = arm.getJ3().getTargetPosition();
+                    j4Pos = arm.getJ4().getTargetPosition();
+                    j5Pos = arm.getJ5().getTargetPosition();
                 }
                 else {
                     arm.rollbackPose();
-                    std::cout << "Hit deadzone!!" << std::endl;
+                    //std::cout << "Hit deadzone!!" << std::endl;
                 }
 
-                const Vector3d ef = arm.getJ2().getCurrentGlobalEndpoint();
+                const Vector3d ef = arm.getJ5().getCurrentGlobalEndpoint();
                 //std::cout << ef << std::endl;
-                //plot.draw(ef);
+                //std::cout << arm.getJ3().getCurrentPosition() << std::endl;
+                plot.draw(ef);
 
                 arm.update(frametime);
 

MotionCore/MainLoop.h

@@ -20,6 +20,6 @@ private:
 	bool isRunning = true;
 	
 	Arm arm;
-	std::array<AxisAlignedBoundingBox, 1> deadzones;
+	std::array<AxisAlignedBoundingBox, 2> deadzones;
 	VKBSimController leftVKB;
 };