implement basic arm class

MotionCore/Arm.cpp

@@ -0,0 +1,89 @@
+#include "Arm.h"
+#include <iostream>
+
+Arm::Arm(const std::wstring& comPort) noexcept:
+	oSerialBus(comPort),
+	j0(
+		oSerialBus,
+		Vector3d::Y,
+		Vector3d::Up,
+		100.0, //mm
+		0.0,
+		125.0,
+		30.0,
+		180.0,
+		0.0,
+		nullptr
+	),
+	j1(
+		oSerialBus,
+		Vector3d::X,
+		Vector3d::Forward,
+		72.0, //mm
+		1.0,
+		48.0,
+		0.0,
+		120.0,
+		0.0,
+		&j0
+	),
+	j2(
+		oSerialBus,
+		Vector3d::X,
+		Vector3d::Forward,
+		95.0, //mm
+		2.0,
+		115.0,
+		0.0,
+		180.0,
+		0.0,
+		&j1
+	)
+{
+	if (!oSerialBus.isOpen()) {
+		std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl;
+		exit(-1);
+	}
+}
+
+bool Arm::assumeHomePose()
+{
+	if (!j0.goHome(0.5)) {
+		return false;
+	}
+	if (!j1.goHome(0.5)) {
+		return false;
+	}
+	if (!j2.goHome(0.5)) {
+		return false;
+	}
+
+	return true;
+}
+
+Arm::~Arm()
+{
+	oSerialBus.close();
+}
+
+bool Arm::assumeHomePoseImmediate()
+{
+	if (!j0.goHomeImmediate()) {
+		return false;
+	}
+	if (!j1.goHomeImmediate()) {
+		return false;
+	}
+	if (!j2.goHomeImmediate()) {
+		return false;
+	}
+
+	return true;
+}
+
+void Arm::update(double frametime)
+{
+	j0.update(frametime);
+	j1.update(frametime);
+	j2.update(frametime);
+}

MotionCore/Arm.h

@@ -0,0 +1,24 @@
+#pragma once
+#include "ArmSegment.h"
+#include <string>
+
+class Arm
+{
+public:
+	Arm(const std::wstring& comPort) noexcept;
+	Arm(const Arm& other) = delete;
+	Arm(Arm&& other) noexcept = default;
+	~Arm();
+	
+	bool assumeHomePoseImmediate();
+	bool assumeHomePose();
+
+	void update(double frametime);
+
+private:
+	OSerialBus oSerialBus;
+	ArmSegment j0;
+	ArmSegment j1;
+	ArmSegment j2;
+};
+

MotionCore/ArmSegment.cpp

@@ -0,0 +1,113 @@
+#include "ArmSegment.h"
+#include "OSerialBus.h"
+
+ArmSegment::ArmSegment(
+	OSerialBus& serialBus,
+	const Vector3d& rotationAxis,
+	const Vector3d& longitudinalAxis,
+	const double length,
+	const uint8_t motorIndex,
+	const double mechanicalToVirtualAngleOffset,
+	const double mechanicalMinAngle,
+	const double mechanicalMaxAngle,
+	const double homeAngle,
+	ArmSegment* parent
+) noexcept :
+	rotationAxis(rotationAxis),
+	longitudinalAxis(longitudinalAxis),
+	length(length),
+	motor(
+		motorIndex,
+		serialBus,
+		virtualToMechanicalAngle(homeAngle),
+		mechanicalMinAngle,
+		mechanicalMaxAngle,
+		0.5
+	),
+	parent(parent)
+{
+	
+}
+
+bool ArmSegment::goHome(const std::optional<double> speed) noexcept
+{
+	return motor.moveTo(
+		virtualToMechanicalAngle(homeAngle),
+		speed
+	);
+}
+
+
+bool ArmSegment::goHomeImmediate() noexcept
+{
+	return motor.moveToImmediate(
+		virtualToMechanicalAngle(homeAngle)
+	);
+}
+
+bool ArmSegment::moveTo(const double theta, const std::optional<double> speed) noexcept
+{
+	return motor.moveTo(
+		virtualToMechanicalAngle(theta),
+		speed
+	);
+}
+
+
+bool ArmSegment::moveToImmediate(const double theta) noexcept
+{
+	return motor.moveToImmediate(
+		virtualToMechanicalAngle(theta)
+	);
+}
+
+bool ArmSegment::moveBy(const double theta, const std::optional<double> speed) noexcept
+{
+	return motor.moveBy(
+		virtualToMechanicalAngle(theta),
+		speed
+	);
+}
+
+
+bool ArmSegment::moveByImmediate(const double theta) noexcept
+{
+	return motor.moveByImmediate(
+		virtualToMechanicalAngle(theta)
+	);
+}
+
+bool ArmSegment::stopMoving() noexcept
+{
+	return motor.stopMoving();
+}
+
+void ArmSegment::pauseMoving() noexcept
+{
+	motor.pauseMoving();
+}
+
+void ArmSegment::resumeMoving() noexcept
+{
+	motor.resumeMoving();
+}
+
+bool ArmSegment::assumeTargetPositionImmediately() noexcept
+{
+	return motor.assumeTargetPositionImmediately();
+}
+
+void ArmSegment::update(const double frametime)
+{
+	motor.update(frametime);
+}
+
+double ArmSegment::virtualToMechanicalAngle(const double virtualAngle)
+{
+	return virtualAngle + mechanicalToVirtualAngleOffset;
+}
+
+double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle)
+{
+	return hardwareAngle - mechanicalToVirtualAngleOffset;
+}

MotionCore/ArmSegment.h

@@ -0,0 +1,66 @@
+#pragma once
+#include "Motor.h"
+#include "Vector3d.h"
+#include <memory>
+
+class OSerialBus;
+
+class ArmSegment
+{
+public:
+	ArmSegment(
+		OSerialBus& serialBus,
+		const Vector3d& rotationAxis,
+		const Vector3d& longitudinalAxis,
+		const double length,
+		const uint8_t motorIndex,
+		const double mechanicalToVirtualAngleOffset, // Mechanical angle at which this segment is neutral to its parent (assuming 0 degrees in virtual space)
+		const double mechanicalMinAngle,
+		const double mechanicalMaxAngle,
+		const double homeAngle,
+		ArmSegment* parent // May be nullptr
+	) noexcept;
+
+	ArmSegment(const ArmSegment& other) = delete;
+	ArmSegment(ArmSegment&& other) noexcept = default;
+
+	// Will return the motor to its home angle
+	bool goHome(const std::optional<double> speed) noexcept;
+	// Will return the motor to its home angle immediately (this is jerky and may damage the hardware long-term)
+	bool goHomeImmediate() noexcept;
+
+	// Will move the motor to the desired angle with ramp-up and controlled speed
+	bool moveTo(const double theta, const std::optional<double> speed) noexcept;
+	// Will move the motor instantly to the desired angle (this is jerky and may damage the hardware long-term)
+	bool moveToImmediate(const double theta) noexcept;
+	// Will move the motor by a specified angle with ramp-up and controlled speed
+	bool moveBy(const double theta, const std::optional<double> speed) noexcept;
+	// Will move the motor by a specified angle instantly (this is jerky and may damage the hardware long-term)
+	bool moveByImmediate(const double theta) noexcept;
+	bool stopMoving() noexcept;
+	void pauseMoving() noexcept;
+	void resumeMoving() noexcept;
+	// Will move the motor to its current target instantly (this is jerky and may damage the hardware long-term)
+	bool assumeTargetPositionImmediately() noexcept;
+
+	void update(const double frametime);
+
+	void setSpeed(const double speed) noexcept { motor.setSpeed(speed); };
+	double getMechanicalMinAngle() const { return motor.getMechanicalMinPosition(); };
+	double getMechanicalMaxAngle() const { return motor.getMechanicalMaxPosition(); };
+	double getSpeed() const noexcept { return motor.getSpeed(); };
+	ArmSegment* getParent() { return parent; };
+	ArmSegment const* getParent() const { return parent; };
+
+private:
+	double virtualToMechanicalAngle(const double virtualAngle);
+	double mechanicalToVirtualAngle(const double hardwareAngle);
+
+	Motor motor;
+	Vector3d rotationAxis;
+	Vector3d longitudinalAxis;
+	double length;
+	double mechanicalToVirtualAngleOffset;
+	double homeAngle;
+	ArmSegment* parent;
+};

MotionCore/MainLoop.cpp

@@ -4,40 +4,9 @@
 #include <chrono>
 
 MainLoop::MainLoop() noexcept:
-    oSerialBus(L"COM4"),
-    j0(
-        0,
-        oSerialBus,
-        0,
-        30.0,
-        180.0,
-        125.0
-    ),
-    j1(
-        1,
-        oSerialBus,
-        0.0,
-        0.0,
-        120.0,
-        51.0
-    ),
-    j2(
-        2,
-        oSerialBus,
-        0.0,
-        0.0,
-        180.0,
-        115.0
-    )
+    arm(L"COM4")
 {
-
-    if (!oSerialBus.isOpen()) {
-        std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl;
-    }
-
-    j0.moveHomeImmediate();
-    j1.moveHomeImmediate();
-    j2.moveHomeImmediate();
+    arm.assumeHomePoseImmediate();
 }
 
 MainLoop& MainLoop::getInstance()
@@ -48,7 +17,6 @@ MainLoop& MainLoop::getInstance()
 
 MainLoop::~MainLoop() noexcept
 {
-    oSerialBus.close();
 }
 
 void MainLoop::run()
@@ -59,9 +27,7 @@ void MainLoop::run()
         std::chrono::steady_clock::time_point begin = std::chrono::high_resolution_clock::now();
         const double frametime = (begin - lastBegin).count() * 10e-7; // in milliseconds
 
-        j0.update(frametime);
-        j1.update(frametime);
-        j2.update(frametime);
+        arm.update(frametime);
 
         lastBegin = std::chrono::high_resolution_clock::now();
     }

MotionCore/MainLoop.h

@@ -1,6 +1,5 @@
 #pragma once
-#include "OSerialBus.h"
-#include "Motor.h"
+#include "Arm.h"
 
 // Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime
 class MainLoop
@@ -15,10 +14,7 @@ public:
 private:
 	MainLoop() noexcept;
 
-	OSerialBus oSerialBus;
-	Motor j0;
-	Motor j1;
-	Motor j2;
-
 	bool isRunning = true;
+	
+	Arm arm;
 };

MotionCore/MotionCore.vcxproj

@@ -143,15 +143,21 @@
     </Link>
   </ItemDefinitionGroup>
   <ItemGroup>
+    <ClCompile Include="Arm.cpp" />
+    <ClCompile Include="ArmSegment.cpp" />
     <ClCompile Include="main.cpp" />
     <ClCompile Include="MainLoop.cpp" />
     <ClCompile Include="Motor.cpp" />
     <ClCompile Include="OSerialBus.cpp" />
+    <ClCompile Include="Vector3d.cpp" />
   </ItemGroup>
   <ItemGroup>
+    <ClInclude Include="Arm.h" />
+    <ClInclude Include="ArmSegment.h" />
     <ClInclude Include="MainLoop.h" />
     <ClInclude Include="Motor.h" />
     <ClInclude Include="OSerialBus.h" />
+    <ClInclude Include="Vector3d.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">

MotionCore/MotionCore.vcxproj.filters

@@ -27,6 +27,15 @@
     <ClCompile Include="Motor.cpp">
       <Filter>Quelldateien</Filter>
     </ClCompile>
+    <ClCompile Include="ArmSegment.cpp">
+      <Filter>Quelldateien</Filter>
+    </ClCompile>
+    <ClCompile Include="Vector3d.cpp">
+      <Filter>Quelldateien</Filter>
+    </ClCompile>
+    <ClCompile Include="Arm.cpp">
+      <Filter>Quelldateien</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="MainLoop.h">
@@ -38,5 +47,14 @@
     <ClInclude Include="Motor.h">
       <Filter>Headerdateien</Filter>
     </ClInclude>
+    <ClInclude Include="ArmSegment.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
+    <ClInclude Include="Vector3d.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
+    <ClInclude Include="Arm.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

MotionCore/Motor.cpp

@@ -12,7 +12,6 @@ Motor::Motor(
 	const double initialPosition,
 	const double mechanicalMinPosition,
 	const double mechanicalMaxPosition,
-	const double homePosition,
 	const double initialSpeed
 ) noexcept:
 	index(index),
@@ -21,8 +20,7 @@ Motor::Motor(
 	targetPosition(currentPosition),
 	moveStartPosition(currentPosition),
 	mechanicalMinPosition(mechanicalMinPosition),
-	mechanicalMaxPosition(mechanicalMaxPosition),
-	homePosition(homePosition)
+	mechanicalMaxPosition(mechanicalMaxPosition)
 {
 	setSpeed(initialSpeed);
 }
@@ -35,7 +33,6 @@ Motor::Motor(Motor&& other) noexcept:
 	moveStartPosition(other.moveStartPosition),
 	mechanicalMinPosition(other.mechanicalMinPosition),
 	mechanicalMaxPosition(other.mechanicalMaxPosition),
-	homePosition(other.homePosition),
 	targetSpeed(other.targetSpeed),
 	speedLow(other.speedLow)
 {
@@ -130,26 +127,6 @@ bool Motor::moveByImmediate(const double theta) noexcept
 	return result;
 }
 
-bool Motor::moveHome(const std::optional<double> speed) noexcept
-{
-	if (disabled) {
-		std::cerr << "Trying to act on disabled motor object!" << std::endl;
-		return false;
-	}
-
-	return moveTo(homePosition, speed);
-}
-
-bool Motor::moveHomeImmediate() noexcept
-{
-	if (disabled) {
-		std::cerr << "Trying to act on disabled motor object!" << std::endl;
-		return false;
-	}
-
-	return moveToImmediate(homePosition);
-}
-
 bool Motor::stopMoving() noexcept
 {
 	if (disabled) {
@@ -202,7 +179,7 @@ void Motor::update(const double frametime)
 		return;
 	}
 
-	handleRampUp(frametime);
+	handleMovement(frametime);
 }
 
 void Motor::setSpeed(const double speed) noexcept
@@ -221,7 +198,7 @@ bool Motor::isPositionInMechanicalRange(const double pos) const noexcept
 	return pos >= mechanicalMinPosition && pos <= mechanicalMaxPosition;
 }
 
-void Motor::handleRampUp(const double frametime)
+void Motor::handleMovement(const double frametime)
 {
 	constexpr double epsilon = 0.1;
 	if (std::abs(currentPosition - targetPosition) > epsilon) {

MotionCore/Motor.h

@@ -11,23 +11,24 @@ public:
 		const double initialPosition,
 		const double mechanicalMinPosition,
 		const double mechanicalMaxPosition,
-		const double homePosition,
 		const double initialSpeed = 0.5
 	) noexcept;
 
 	Motor(const Motor& other) = delete;
 	Motor(Motor&& other) noexcept;
 
-	// Will move the motor immediately to the desired position (don't do that)
-	bool moveToImmediate(const double theta) noexcept;
+	// Will move the motor to the desired position with ramp-up and controlled speed
 	bool moveTo(const double theta, const std::optional<double> speed) noexcept;
+	// Will move the motor instantly to the desired position (this is jerky and may damage the hardware long-term)
+	bool moveToImmediate(const double theta) noexcept;
+	// Will move the motor by a specified angle with ramp-up and controlled speed
 	bool moveBy(const double theta, const std::optional<double> speed) noexcept;
+	// Will move the motor by a specified angle instantly (this is jerky and may damage the hardware long-term)
 	bool moveByImmediate(const double theta) noexcept;
-	bool moveHome(const std::optional<double> speed) noexcept;
-	bool moveHomeImmediate() noexcept;
 	bool stopMoving() noexcept;
 	void pauseMoving() noexcept;
 	void resumeMoving() noexcept;
+	// Will move the motor to its current target instantly (this is jerky and may damage the hardware long-term)
 	bool assumeTargetPositionImmediately() noexcept;
 
 	void update(const double frametime);
@@ -39,12 +40,13 @@ public:
 	void setSpeed(const double speedTarget) noexcept;
 	double getMechanicalMinPosition() const { return mechanicalMinPosition; };
 	double getMechanicalMaxPosition() const { return mechanicalMaxPosition; };
-	double getHomePosition() const { return homePosition; };
 	double getPaused() const { return paused; };
 	bool isPositionInMechanicalRange(const double pos) const noexcept;
 
 private:
-	void handleRampUp(const double frametime);
+	// Update-handler for moving
+	void handleMovement(const double frametime);
+	// Sends a move absolute command to the serial busses MCU
 	bool sendMACommand(const double targetPos) noexcept;
 	// Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition
 	double calculateCurrentSpeed() const noexcept;
@@ -58,7 +60,6 @@ private:
 	double speedLow; // End- and startpoint of ramp-up phase
 	const double mechanicalMinPosition;
 	const double mechanicalMaxPosition;
-	const double homePosition;
 	bool disabled = false;
 	bool paused = false;
 };

MotionCore/Vector3d.cpp

@@ -0,0 +1,160 @@
+#include "Vector3d.h"
+#include <ostream>
+
+const Vector3d Vector3d::Zero       { 0.0,  0.0,  0.0 };
+const Vector3d Vector3d::One        { 1.0,  1.0,  1.0 };
+const Vector3d Vector3d::Forward    { 0.0,  0.0,  1.0 };
+const Vector3d Vector3d::Backward   { 0.0,  0.0, -1.0 };
+const Vector3d Vector3d::Up         { 0.0,  1.0,  0.0 };
+const Vector3d Vector3d::Down       { 0.0, -1.0,  0.0 };
+const Vector3d Vector3d::Left       { -1.0,  0.0,  0.0 };
+const Vector3d Vector3d::Right      { 1.0,  0.0,  0.0 };
+const Vector3d Vector3d::X          { 1.0,  0.0,  0.0 };
+const Vector3d Vector3d::Y          { 0.0,  1.0,  0.0 };
+const Vector3d Vector3d::Z          { 0.0,  0.0,  1.0 };
+
+Vector3d::Vector3d()
+    : x(0.0), y(0.0), z(0.0)
+{
+}
+
+Vector3d::Vector3d(double x, double y, double z)
+    : x(x), y(y), z(z)
+{
+}
+
+Vector3d& Vector3d::operator=(const Vector3d& other)
+{
+    if (this != &other)
+    {
+        x = other.x;
+        y = other.y;
+        z = other.z;
+    }
+    return *this;
+}
+
+Vector3d& Vector3d::operator=(Vector3d&& other) noexcept
+{
+    if (this != &other)
+    {
+        x = other.x;
+        y = other.y;
+        z = other.z;
+    }
+    return *this;
+}
+
+Vector3d Vector3d::operator+(const Vector3d& rhs) const
+{
+    return { x + rhs.x, y + rhs.y, z + rhs.z };
+}
+
+Vector3d Vector3d::operator-(const Vector3d& rhs) const
+{
+    return { x - rhs.x, y - rhs.y, z - rhs.z };
+}
+
+Vector3d Vector3d::operator*(double scalar) const
+{
+    return { x * scalar, y * scalar, z * scalar };
+}
+
+Vector3d Vector3d::operator/(double scalar) const
+{
+    return { x / scalar, y / scalar, z / scalar };
+}
+
+Vector3d& Vector3d::operator+=(const Vector3d& rhs)
+{
+    x += rhs.x;
+    y += rhs.y;
+    z += rhs.z;
+    return *this;
+}
+
+Vector3d& Vector3d::operator-=(const Vector3d& rhs)
+{
+    x -= rhs.x;
+    y -= rhs.y;
+    z -= rhs.z;
+    return *this;
+}
+
+Vector3d& Vector3d::operator*=(double scalar)
+{
+    x *= scalar;
+    y *= scalar;
+    z *= scalar;
+    return *this;
+}
+
+Vector3d& Vector3d::operator/=(double scalar)
+{
+    x /= scalar;
+    y /= scalar;
+    z /= scalar;
+    return *this;
+}
+
+bool Vector3d::operator==(const Vector3d& rhs) const
+{
+    return x == rhs.x && y == rhs.y && z == rhs.z;
+}
+
+bool Vector3d::operator!=(const Vector3d& rhs) const
+{
+    return !(*this == rhs);
+}
+
+std::ostream& operator<<(std::ostream& os, const Vector3d& v)
+{
+    os << "Vector3d(" << v.x << ", " << v.y << ", " << v.z << ")";
+    return os;
+}
+
+double Vector3d::dot(const Vector3d& rhs) const
+{
+    return x * rhs.x + y * rhs.y + z * rhs.z;
+}
+
+double Vector3d::magnitude() const
+{
+    return std::sqrt(x * x + y * y + z * z);
+}
+
+double Vector3d::magnitudeSquared() const
+{
+    return x * x + y * y + z * z;
+}
+
+Vector3d Vector3d::cross(const Vector3d& rhs) const
+{
+    return {
+        y * rhs.z - z * rhs.y,
+        z * rhs.x - x * rhs.z,
+        x * rhs.y - y * rhs.x
+    };
+}
+
+Vector3d Vector3d::normalized() const
+{
+    double m = magnitude();
+    if (m == 0.0)
+        return Vector3d::Zero;
+
+    double inv = 1.0 / m;
+    return { x * inv, y * inv, z * inv };
+}
+
+void Vector3d::normalize()
+{
+    double m = magnitude();
+    if (m == 0.0)
+        return;
+
+    double inv = 1.0 / m;
+    x *= inv;
+    y *= inv;
+    z *= inv;
+}

MotionCore/Vector3d.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <iosfwd>
+
+class Vector3d
+{
+public:
+    double x;
+    double y;
+    double z;
+
+public:
+    Vector3d();
+    Vector3d(double x, double y, double z);
+
+    Vector3d(const Vector3d& other) = default;
+    Vector3d(Vector3d&& other) noexcept = default;
+
+
+    double dot(const Vector3d& rhs) const;
+    double magnitude() const;
+    double magnitudeSquared() const;
+    Vector3d cross(const Vector3d& rhs) const;
+    Vector3d normalized() const;
+    void normalize();
+
+    Vector3d& operator=(const Vector3d& other);
+    Vector3d& operator=(Vector3d&& other) noexcept;
+
+    Vector3d operator+(const Vector3d& rhs) const;
+    Vector3d operator-(const Vector3d& rhs) const;
+
+    Vector3d operator*(double scalar) const;
+    Vector3d operator/(double scalar) const;
+
+    Vector3d& operator+=(const Vector3d& rhs);
+    Vector3d& operator-=(const Vector3d& rhs);
+
+    Vector3d& operator*=(double scalar);
+    Vector3d& operator/=(double scalar);
+
+    bool operator==(const Vector3d& rhs) const;
+    bool operator!=(const Vector3d& rhs) const;
+
+public:
+    static const Vector3d Zero;
+    static const Vector3d One;
+    static const Vector3d Forward;
+    static const Vector3d Backward;
+    static const Vector3d Up;
+    static const Vector3d Down;
+    static const Vector3d Left;
+    static const Vector3d Right;
+    static const Vector3d X;
+    static const Vector3d Y;
+    static const Vector3d Z;
+};
+
+std::ostream& operator<<(std::ostream& os, const Vector3d& v);