Added starting comments for all codes. Updated the README.md.

README.md

@@ -1 +1,150 @@
-Repository of Introduction to Robotics Lab Robotic arm.
+# Overview
+
+Each of the following codes allows a specific directive to be sent to a 2D planar robot:
+
+**[jointControl](#jointControl)**: Drives the mBot robot to a target location (x, y, theta) using gyroscopic data for precise navigation and orientation tracking.
+
+**[cartControl](#cartControl)**: Drives the mBot robot to a target location (x, y, theta) using gyroscopic data for precise navigation and orientation tracking.
+
+**[testKinematics](#testKinematics)**: Retrieves and prints real-time X, Y, and Z angles from the gyro sensor, enabling orientation monitoring.
+
+**[linearControl](#linearControl)**: Implements basic movement patterns such as moving forward, backward, and turning left or right for foundational motion control.
+
+**[drawImage](#drawImage)**: Simulates Roomba-like behavior by integrating ultrasonic sensors for obstacle detection and avoidance during autonomous navigation.
+
+**[linearControlFurther](#linearControlFurther)**: Implements line-following behavior using a line-finder module to detect and respond to black lines on the ground for path tracking.
+
+# Repository Structure
+
+### jointControl/
+
+* @file      jointControl.cpp
+* @author    Guillaume Gibert
+* @version   -
+* @date      2025/04/13
+* @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
+* @functions :
+        - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main()
+
+### cartControl/
+ * @file      cartControl.cpp
+ * @author    Charles STELANDRE
+ * @version   V1
+ * @date      2025/04/13
+ * @brief     Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
+ * @functions :
+ * @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+          - void closeRobot(DynamixelHandler& dxlHandler)
+          - int main(int argc, char** argv)
+ * Input:
+ * - L1 : Length of the first link (cm)
+ * - L2 : Length of the second link (cm)
+ * - x : x-component of target position (cm)
+ * - y : y-component of target position (cm)
+ *
+ * Output:
+ * - Displacement of the robot using cartesian coordinates.
+
+
+### testKinematics/
+
+* @file      testKinematics.cpp
+* @author    Guillaume Gibert
+* @version   -
+* @date      2025/04/13
+* @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
+* @functions :
+        - bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
+        - bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
+        - int main()
+
+
+ ### linearControl/
+ * @file      linearControl.cpp
+ * @author    Guillaume Gibert
+ * @version   -
+ * @date      2025/04/13
+ * @brief     Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
+ * @functions :
+ * @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+          - void closeRobot(DynamixelHandler& dxlHandler)
+          - int main(int argc, char** argv)
+* Input:
+ * - L1 : Length of the first link (cm)
+ * - L2 : Length of the second link (cm)
+ * - x : x-component of target position (cm)
+ * - y : y-component of target position (cm)
+ *
+ * Output:
+ * - Displacement of the robot using cartesian coordinates.
+
+### drawImage/
+
+ * @file      drawImage.cpp
+ * @author    Guillaume Gibert
+ * @version   -
+ * @date      2025/04/13
+ * @brief     Description : This file was provided by G. Gibert. It allows to use image processing to draw an image after converting it to correct dimensions.
+ * @functions :
+ * @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main(int argc, char** argv)
+ * Input:
+ * - L1 : Length of the first link (cm)
+ * - L2 : Length of the second link (cm)
+ * - image : directory of an image to use for image drawing
+
+### linearControlFurther/
+
+ * @file      linearControl.cpp
+ * @author    Guillaume Gibert
+ * @version   -
+ * @date      2025/04/13
+ * @brief     Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
+ * @functions :
+ *       - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+         - void closeRobot(DynamixelHandler& dxlHandler)
+         - int main(int argc, char** argv)
+* Input:
+ * - L1 : Length of the first link (cm)
+ * - L2 : Length of the second link (cm)
+ * - x : x-component of target position (cm)
+ * - y : y-component of target position (cm)
+
+# README.md
+
+Documentation for the repository.
+
+# .DS_Store
+
+System file (can be ignored or removed from the repository).
+
+# Requirements
+
+Hardware: mBot robot kit
+
+Software: Arduino IDE
+
+Libraries: Ensure required libraries (e.g., MeMCore.h) are installed in Arduino IDE.
+
+# Installation
+
+Clone this repository:
+
+git clone https://gitarero.ecam.fr/charles.stelandre/IntroRoboticsLab2.git
+
+Open Arduino IDE and navigate to the folder containing the .ino file you want to use.
+
+Install any necessary libraries via Arduino Library Manager.
+
+Upload the code to your mBot using a USB connection.
+
+# Usage
+
+Navigate to the folder corresponding to the desired functionality (e.g., gyro/).
+
+Open the .ino file in Arduino IDE.
+
+Follow any instructions provided in the comments within the code to execute experiments.

src/Kinematics.cpp

@@ -1,6 +1,20 @@
+/*
+ @file      Kinematics.cpp
+ @author    Charles STELANDRE
+ @version   V1
+ @date      2025/04/13
+ @brief     Description : This file contains all kinematics computations for FK, IK and DK.
+ @functions :
+ @      - float deg2rad(float angle)
+        - float rad2deg(float angle)
+        - std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
+        - std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
+        - std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
+        - int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
+        - cv::Mat  computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
+ */
 #include "Kinematics.h"
 
-
 float deg2rad(float angle)
 {
     //Converts degrees to radians
@@ -9,18 +23,18 @@ float deg2rad(float angle)
 
 float rad2deg(float angle)
 {
-    //Converts radians to degrees
+    // Converts radians to degrees
 	return angle*180.0/M_PI;
 }
 
 std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
 {
-    //Computes the x,y coordinates
+    // Computes the x,y coordinates
 	float x = L1 * cos(q1) + L2 * cos(q1+q2);
 	float y = L1 * sin(q1) + L2 * sin(q1+q2);
-	//Printing each joint value to get the target x,y
+	// Printing each joint value to get the target x,y
     std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl;
-	//Declare a vector X containing target x,y
+	// Declare a vector X containing target x,y
     std::vector<float> X;
 	X.push_back(x);
 	X.push_back(y);
@@ -30,30 +44,30 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
 
 std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
 {
-    //declares an empty vector qi
+    // Declares an empty vector qi
 	std::vector<float> qi;
 	
-    //From the equations, computes the cosine(q2) to be used to find the number of solutions
+    // From the equations, computes the cosine(q2) to be used to find the number of solutions
 	float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
 	
 	std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
-	//Checks if there is no solution (out of reach)
+	// Checks if there is no solution (out of reach)
 	if (cos_q2 >1 | cos_q2 <-1)
 	{
-        //Fills the first value of qi with a value qi[0]=0.0, indicating no solution
+        // Fills the first value of qi with a value qi[0]=0.0, indicating no solution
 		qi.push_back(0.0);
 		std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
 	}
-    //Checks if there is one solution exactly
+    // Checks if there is one solution exactly
 	else if (cos_q2 == 1)
 	{
-        //Fills the first value of qi with a value qi[0]=1.0, indicating a single solution
+        // Fills the first value of qi with a value qi[0]=1.0, indicating a single solution
 		qi.push_back(1.0);
 		float q1 = atan2(y, x);
-        //q2=0 means arm is extended, consistantly with cos_q2==1
+        // q2=0 means arm is extended, consistantly with cos_q2==1
 		float q2 = 0;
 		std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
-        //Adds q1 and q2 to qi
+        // Adds q1 and q2 to qi
 		qi.push_back(q1);
 		qi.push_back(q2);
 	}
@@ -61,32 +75,32 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
 	{
 		qi.push_back(1.0);
 		float q1 = atan2(y, x);
-        //q2=0 means arm is folded on itself, consistantly with cos_q2==-1
+        // q2=0 means arm is folded on itself, consistantly with cos_q2==-1
 		float q2 = M_PI;
 		std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
-        //Adds q1 and q2 to qi
+        // Adds q1 and q2 to qi
 		qi.push_back(q1);
 		qi.push_back(q2);
 	}
 	else
 	{
-        //Fills the first value of qi with a value qi[0]=2.0, indicating two solutions
+        // Fills the first value of qi with a value qi[0]=2.0, indicating two solutions
 		qi.push_back(2.0);
 		std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
-		//Computes the joint values
+		// Computes the joint values
 		float q2 = acos(cos_q2);
 		float q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
 		std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
-        //Adds q1 and q2 to qi
+        // Adds q1 and q2 to qi
 		qi.push_back(q1);
 		qi.push_back(q2);
 		
-		//Compute the second solution
+		// Compute the second solution
 		q2 = -acos(cos_q2);
 		q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
 		
 		std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
-        //Adds the second solution at the end of vector qi.
+        // Adds the second solution at the end of vector qi.
 		qi.push_back(q1);
 		qi.push_back(q2);
 	}
@@ -96,14 +110,14 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
 
 std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
 {
-    //Creates a vector jacobian
+    // Creates a vector jacobian
 	std::vector<float> jacobian;
-	//Computes the jacobian's coefficients
+	// Computes the jacobian's coefficients
 	float j11 = -L2*sin(q1+q2) - L1*sin(q1);
 	float j12 = -L2*sin(q1+q2);
 	float j21 = L2*cos(q1+q2) + L1*cos(q1); 
 	float j22 = L2*cos(q1+q2);
-	//Adds each coefficient to the jacobian vector
+	// Adds each coefficient to the jacobian vector
 	jacobian.push_back(j11);
 	jacobian.push_back(j12);
 	jacobian.push_back(j21);

src/cartControl.cpp

@@ -1,3 +1,15 @@
+/*
+ @file      cartControl.cpp
+ @author    Charles STELANDRE
+ @version   V1
+ @date      2025/04/13
+ @brief     Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
+ @functions :
+ @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main(int argc, char** argv)
+ */
+
 #include <chrono>
 #include <thread>
 

src/drawImage.cpp

@@ -1,3 +1,15 @@
+/*
+ @file      drawImage.cpp
+ @author    Guillaume Gibert
+ @version   -
+ @date      2025/04/13
+ @brief     Description : This file was provided by G. Gibert. It allows to use image processing to draw an image after converting it to correct dimensions.
+ @functions :
+ @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main(int argc, char** argv)
+ */
+
 #include <chrono>
 #include <thread>
 
@@ -73,4 +85,4 @@ int main(int argc, char** argv)
 	}
 		
 	return 0;
-}
+}

src/jointControl.cpp

@@ -1,3 +1,15 @@
+/*
+ @file      jointControl.cpp
+ @author    Guillaume Gibert
+ @version   -
+ @date      2025/04/13
+ @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
+ @functions :
+        - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main()
+ */
+
 #include <chrono>
 #include <thread>
 
@@ -71,4 +83,4 @@ int main(int argc, char** argv)
 	}
 		
 	return 0;
-}
+}

src/linearControl.cpp

@@ -1,3 +1,15 @@
+/*
+ @file      linearControl.cpp
+ @author    Guillaume Gibert
+ @version   -
+ @date      2025/04/13
+ @brief     Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
+ @functions :
+ @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main(int argc, char** argv)
+ */
+
 #include <chrono>
 #include <thread>
 

src/linearControlFurther.cpp

@@ -1,14 +1,27 @@
+/*
+ @file      linearControl.cpp
+ @author    Guillaume Gibert, Charles Stelandre
+ @version   -
+ @date      2025/04/13
+ @brief     Description : This file was provided by G. Gibert, and completed by C.Stelandre. It allows to use a proportionally controlled system to join two points in a linear trajectory, while checking for the validity of a the target point.
+ @functions :
+ @      - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+        - void closeRobot(DynamixelHandler& dxlHandler)
+        - int main(int argc, char** argv)
+ */
+
 #include <chrono>
 #include <thread>
 
 #include "Kinematics.h"
 #include "DynamixelHandler.h"
 
-// TO BE ADJUSTED IF NECESSARY
+// TO BE ADJUSTED FOR TESTINGS
 float _fDistanceThreshold = 0.1f; // in cm
 float _pController = 0.1f;
 float _fps = 20.0f; // in Hz
 float det_threshold = 1e-6;
+
 // Dynamixel config infos
 DynamixelHandler _oDxlHandler;
 std::string _robotDxlPortName = "/dev/ttyUSB0";
@@ -73,9 +86,11 @@ int main(int argc, char** argv)
 			// Computes Jacobian matrix
 			std::vector<float> vJacobianMatrix = computeDifferentialKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2);
 			
+            // Computes the determinant of the Jacobian
 			float det = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
 			cv::Mat vInverseJacobianMatrix;
-			if (det < det_threshold){
+			// Checking if the determinant is within valid range.
+            if (det < det_threshold){
 				// Prints an error if close to a singularity.
 				std::cerr << "[WARNING] Jacobian determinant is close to 0 !(" << det << "(, potential singularity." << std::endl;
 				// If there is a singularity, return identity matrix (do not move)
@@ -110,10 +125,7 @@ int main(int argc, char** argv)
 			std::vector<float> vTargetJointVelocity;
 				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[0]); 
 				vTargetJointVelocity.push_back(0.0f); // pen lift up/down
-				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]); 
+				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]);
-			// Sends the target joint position to actuate the robot
-			//_oDxlHandler.sendTargetJointVelocity(vTargetJointVelocity);
-			// VELOCITY CONTROL qdot
 			
 			// Waits 1000/fps ms
 			std::this_thread::sleep_for(std::chrono::milliseconds((long int)(1000.0f/_fps)));

src/testKinematics.cpp

@@ -1,7 +1,17 @@
+/*
+ @file      testKinematics.cpp
+ @author    Guillaume Gibert
+ @version   -
+ @date      2025/04/13
+ @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
+ @functions :
+        - bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
+        - bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
+        - int main()
+ */
+
 #include <iostream>
-
 #include "Kinematics.h"
-
 #define ERROR_THRESHOLD 0.00001
 
 std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);
@@ -119,4 +129,4 @@ int main()
 		std::cout << "FAILED!" << std::endl;
 	
 	return 0;
-}
+}