Diff: main.cpp

Revision:

11:ca91fc47ad02

Parent:

7:08fd3bc7a3cf

Child:

16:29d3851cfd52

--- a/main.cpp	Fri Oct 11 13:40:36 2019 +0000
+++ b/main.cpp	Mon Oct 21 14:20:28 2019 +0000
@@ -23,6 +23,10 @@
 float motorvalue;
 double u_i;
 float potmeter1 = pot1.read();
+float yendeffector = 20;
+float xendeffector = 0;
+float Kp;
+
 
 
 
@@ -37,9 +41,9 @@
     static double error_integral = 0;
     static double error_prev = error;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-    double Kp = pot1.read()*10;
-    double Ki = 0.1;
-    double Kd = 0.1;
+    float Kp = 30;
+    float Ki = 2;
+    float Kd = 0.6;
     
     //Proportional part:
     double u_k = Kp * error;
@@ -75,8 +79,21 @@
 //get the reference of the absolutemotorvalueocity
 double getreferenceposition()
 {
+    float L0 = 1.95;
+    float L1=15;
+    float L2=20;
+    float motorcounts1;
+    float tangens = atan2(yendeffector,(L0+xendeffector))*180/3.1415;
+    float cosinus = 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;
+    motorcounts1=tangens+cosinus;
+    //printf("motorcounts1 is %f\r\n", motorcounts1);
+    float tangens2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415;
+    float cosinus2 = 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;
+    //motorcounts2=tangens2+cosinus2;
     float referenceposition;
-    referenceposition = -1+2*pot2.read(); //this determines the amount of spins
+    float variable;
+    variable = motorcounts1/360;
+    referenceposition = variable; //this determines the amount of spins
     return referenceposition;
 }   
 
@@ -94,7 +111,7 @@
 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
 void measureandcontrol(void)
 {
-    float referenceposition = getreferenceposition();
+    float referenceposition = getreferenceposition(); 
     measuredposition = getmeasuredposition();
     motorvalue = PID_controller(referenceposition - measuredposition);
     sendtomotor(motorvalue);
@@ -113,6 +130,23 @@
         pc.printf("motorvalue is %f\r\n", motorvalue);
         pc.printf("u_i is %d\r\n", u_i);
         pc.printf("potmeter 1 gives %f\r\n", potmeter1);
+        pc.printf("x is %f\r\n",xendeffector);
+        pc.printf("y is %f\r\n",yendeffector);
+        pc.printf("Kp is %f\r\n", Kp);
+        
+            
+    char a = pc.getc();
+    
+    if(a == 'r'){
+        xendeffector=xendeffector+5;}
+        else if(a=='l'){
+            xendeffector=xendeffector-5;}
+        else if(a=='u'){
+            yendeffector=yendeffector+5;}
+        else if(a=='d'){
+            yendeffector=yendeffector-5;}
+            
+        
         
         char c = pc.getcNb();