Diff: main.cpp

Revision:

24:3c9ac44890f0

Parent:

23:b0222fa7c131

Child:

25:2cc29fa67345

--- a/main.cpp	Fri Oct 25 09:52:56 2019 +0000
+++ b/main.cpp	Fri Oct 25 12:25:48 2019 +0000
@@ -295,6 +295,24 @@
             motory_on=true;
         } 
     }
+    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
+    float y_pos=tan(theta1)*x_pos;
+    if(x_pos>=0.35&&v_refx>0)
+    {
+        v_refx=0;
+    }
+    else if(x_pos<=0.0&&v_refx<0)
+    {
+        v_refx=0;
+    }
+    if(y_pos>=0.45&&v_refy>0)
+    {
+        v_refy=0;
+    }
+    else if(y_pos<=0.07&&v_refy<0)
+    {
+        v_refy=0;
+    }
 }
 
 void error(void){
@@ -315,6 +333,22 @@
     {
         theta1_ref = theta1_old+theta1_dot*T;
         theta2_ref = theta2_old+theta2_dot*T;
+        if(theta1_ref<0.1f)
+        {
+            theta1_ref=0.1f;
+        }
+        else if(theta1_ref<1.6f)
+        {
+            theta1_ref=1.6f;
+        }
+        if(theta2_ref<0.1f)
+        {
+            theta2_ref=0.1f;
+        }
+        else if(theta2_ref<1.6f)
+        {
+            theta2_ref=1.6f;
+        }
     }
     else if(currentState==Homing)
     {
@@ -439,8 +473,8 @@
     get_angles();
     scope.set(0,P0);
     scope.set(1,P1);
-    scope.set(2,theta1);
-    scope.set(3,theta2);
+    scope.set(2,v_refx);
+    scope.set(3,v_refy);
     scope.send(); 
     switch(currentState)
     {
@@ -492,14 +526,6 @@
             break;
     }
     //Preventing the machine from breaking
-    if(theta1>=1.6f)
-    {
-        dir1=1;
-    }
-    if(theta2>=1.6f)
-    {
-        dir2=2;
-    }
 }
 
 void error_occur()