From 42ab8a308b26334e7e0d2ba3fd1a00272de24d4d Mon Sep 17 00:00:00 2001
From: Thomas Olive <thomas.olive@ecam.fr>
Date: Tue, 21 Dec 2021 18:01:58 +0100
Subject: [PATCH] added some comments

---
 DrawObstacles.m |  7 ++++---
 buildPRM.m      | 10 +++++-----
 planPathPRM.m   |  2 +-
 setParams.m     |  2 +-
 4 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/DrawObstacles.m b/DrawObstacles.m
index c04c445..0c350ab 100644
--- a/DrawObstacles.m
+++ b/DrawObstacles.m
@@ -16,7 +16,7 @@ function DrawObstacles()
 % 18/12/2021
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-  [S, G, randVmin, randVmax, L1, L2] = setParams();
+  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
   qi = [];
   q1_plot = [];
   q2_plot = [];
@@ -26,14 +26,15 @@ function DrawObstacles()
   for i=1:10000
     x = -(L1+L2) + rand()*2*(L1+L2); %creating random x and y values
     y = -(L1+L2) + rand()*2*(L1+L2);
-    if((y >= L1)|| (y<=-L1) || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) % keeping them if they are inside the obstacles
+    if(y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))% keeping them if they are inside the obstacles
+      
       [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y);% computing their corresponding q1 q2 coordinates
       for j=1:nbSol
         % saving the computed coordinates
         q1 = qi(j,1);
         q2 = qi(j,2);
         if q2 > 180
-          q2 = q2-360;
+          q2 = q2-360; %the IK function is returning q2 values between 0 and 360
         endif
         q1_plot = [q1_plot q1];
         
diff --git a/buildPRM.m b/buildPRM.m
index 7c69d36..12a76fd 100644
--- a/buildPRM.m
+++ b/buildPRM.m
@@ -34,12 +34,12 @@ function [nbNodes, obstacle, points] = buildPRM()
   threshold = 0.000001;
   
   [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
-  Sq1 = qi(1,1)
-  Sq2 = qi(1,2)
+  Sq1 = qi(1,1);
+  Sq2 = qi(1,2);
   
   [nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
-  Gq1 = qi(1,1)
-  Gq2 = qi(1,2)
+  Gq1 = qi(1,1);
+  Gq2 = qi(1,2);
   
   points = [Sq1 Sq2 S(1) S(2)]';
   obstacle = [];
@@ -59,7 +59,7 @@ function [nbNodes, obstacle, points] = buildPRM()
     x= jTee(1,4);
     y= jTee(2,4);
     %Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
-    if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))
+    if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold)))
     
       n=columns(points);
       
diff --git a/planPathPRM.m b/planPathPRM.m
index d6f7024..797e873 100644
--- a/planPathPRM.m
+++ b/planPathPRM.m
@@ -13,7 +13,7 @@ function planPathPRM()
 % Thomas OLIVE (thomas.olive@ecam.fr)
 % 18/12/2021
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams()
+  [S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
   
   [nbNode, visGraph, points] = buildPRM();
   [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
diff --git a/setParams.m b/setParams.m
index d03aeb4..3755df8 100644
--- a/setParams.m
+++ b/setParams.m
@@ -28,5 +28,5 @@ function [S, G, randVmin, randVmax, L1, L2, threshold] = setParams()
   randVmax = 180;
   L1 = 2;
   L2 = 1;
-  threshold = 0.000001;
+  threshold =0.000001;
 endfunction
\ No newline at end of file