Diff: main.cpp

Revision:

21:245c676f9d72

Parent:

20:e20c26a1f6ba

--- a/main.cpp	Fri Oct 25 14:19:01 2019 +0000
+++ b/main.cpp	Tue Oct 29 14:55:38 2019 +0000
@@ -39,6 +39,8 @@
 float xendeffector = 0;
 float potmeter1 = 5+pot1.read()*60;
 float potmeter2 = pot2.read()*60;
+    float desiredmotorangle1;
+    float desiredmotorangle2;
 
 // ANTI WIND UP NEEDED
 
@@ -132,18 +134,32 @@
     float L2=20;
     
     //Define the new counts that are needed
-    float desiredmotorangle1;
-    float desiredmotorangle2;
+//    float desiredmotorangle1;
+//    float desiredmotorangle2;
     
     //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
-    desiredmotorangle1 = atan2(yendeffector,(L0+xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
-    desiredmotorangle2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
+    desiredmotorangle1 = (atan2(yendeffector,(L0+xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415)-180;
+    desiredmotorangle2 = (atan2(yendeffector,(L0-xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415)-180;
 
+    //Apply boundaries of the motor angles
+//    if (desiredmotorangle1 < -125){
+//        desiredmotorangle1 = -124.9;
+//    }
+//    if (desiredmotorangle1 > 28){
+//        desiredmotorangle1 = 27.9;
+//    }    
+//    if (desiredmotorangle2 > 46){
+//        desiredmotorangle2 = 45.9;
+//    }    
+//    if (desiredmotorangle2 < -130){
+//        desiredmotorangle2 = -129.9;
+//    }
+    
     //Convert motor angles to counts
     float desiredmotorrounds1;
     float desiredmotorrounds2;
-    desiredmotorrounds1 = (desiredmotorangle1-180)/360;
-    desiredmotorrounds2 = (desiredmotorangle2-180)/360;
+    desiredmotorrounds1 = (desiredmotorangle1)/360;
+    desiredmotorrounds2 = (desiredmotorangle2)/360;
     
     //Assign this to new variables because otherwise it doesn't work
     referenceposition1 = desiredmotorrounds1;
@@ -206,22 +222,63 @@
         wait(0.01);
         float potmeter1 = 5+pot1.read()*60;
         float potmeter2 = pot2.read()*60;
-        pc.printf("Kp gives %f\r\n", potmeter1);
-        pc.printf("Ki gives %f\r\n", potmeter2);
+//        pc.printf("Kp gives %f\r\n", potmeter1);
+//        pc.printf("Ki gives %f\r\n", potmeter2);
         pc.printf("x is %f\r\n",xendeffector);
-        pc.printf("y is %f\r\n",yendeffector);        
+        pc.printf("y is %f\r\n",yendeffector);   
+        pc.printf("motorangle1 is%f\r\n",desiredmotorangle1);
+        pc.printf("motorangle2 is%f\r\n",desiredmotorangle2);
         
+        float ditmoetkleinerzijndan = pow((xendeffector-1.95),2)+pow(yendeffector,2);
+        float dit = 28*28;
+        
+        float ditmoetkleinerzijndan2 = pow((xendeffector-1.95),2)+pow(yendeffector,2);
+        float dit2 = 28*28;
+        
+ 
+
         // Control position    
         char a = pc.getcNb();
-    
-        if(a == 'r' && xendeffector < 14){
-            xendeffector=xendeffector+1;}
-        else if(a=='l' && xendeffector > -14){
-            xendeffector=xendeffector-1;}
-        else if(a=='u' && yendeffector < 30){
-            yendeffector=yendeffector+1;}
-        else if(a=='d' && yendeffector > 5){
-            yendeffector=yendeffector-1;}
+        if(xendeffector > 1.95){
+            if(a == 'r' && ditmoetkleinerzijndan < dit){
+                xendeffector=xendeffector+1;}
+            else if(a=='l'){
+                xendeffector=xendeffector-1;}
+            else if(a=='u' && ditmoetkleinerzijndan2 < dit2){
+                yendeffector=yendeffector+1;}
+            else if(a=='d'){
+                yendeffector=yendeffector-1;}
+        }
+        else if( 0 < xendeffector < 1.95){
+            if(a == 'r'){
+                xendeffector=xendeffector+1;}
+            else if(a=='l' && pow(xendeffector-1.95,2)+pow(yendeffector,2) < 28*28){
+                xendeffector=xendeffector-1;}
+            else if(a=='u' && pow(xendeffector-1.95,2)+pow(yendeffector,2) < 28*28){
+                yendeffector=yendeffector+1;}
+            else if(a=='d'){
+                yendeffector=yendeffector-1;}            
+        }
+        else if( -1.95 < xendeffector < 0){
+            if(a == 'r' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
+                xendeffector=xendeffector+1;}
+            else if(a=='l'){
+                xendeffector=xendeffector-1;}
+            else if(a=='u' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
+                yendeffector=yendeffector+1;}
+            else if(a=='d'){
+                yendeffector=yendeffector-1;}            
+        }
+        else if( xendeffector < -1.95){
+            if(a == 'r'){
+                xendeffector=xendeffector+1;}
+            else if(a=='l' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
+                xendeffector=xendeffector-1;}
+            else if(a=='u' && pow(xendeffector+1.95,2)+pow(yendeffector,2) < 28*28){
+                yendeffector=yendeffector+1;}
+            else if(a=='d'){
+                yendeffector=yendeffector-1;}            
+        }        
         
         // Go back to starting position    
         if (but1.read() == 0) {