Diff: main.cpp

Revision:

20:e20c26a1f6ba

Parent:

16:29d3851cfd52

Child:

21:245c676f9d72

--- a/main.cpp	Tue Oct 22 15:05:12 2019 +0000
+++ b/main.cpp	Fri Oct 25 14:19:01 2019 +0000
@@ -8,6 +8,10 @@
 // To play with buttons and potmeters
 AnalogIn pot2(A0);
 AnalogIn pot1(A1);
+DigitalIn but1(D2);
+DigitalIn but2(D3);
+DigitalIn but3(SW2);
+
 
 // Usual stuff
 MODSERIAL pc(USBTX, USBRX);
@@ -33,6 +37,8 @@
 float motorvalue2;
 float yendeffector = 10.6159;
 float xendeffector = 0;
+float potmeter1 = 5+pot1.read()*60;
+float potmeter2 = pot2.read()*60;
 
 // ANTI WIND UP NEEDED
 
@@ -51,9 +57,9 @@
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
     
     // PID variables: we assume them to be the same for both motors
-    float Kp = 10;
-    float Ki = 1;
-    float Kd = 0.01;
+    float Kp = 64.9;
+    float Ki = 3.64;
+    float Kd = 5;
     
     //Proportional part:
     double u_k1 = Kp * error1;
@@ -82,9 +88,9 @@
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
     
     // PID variables: we assume them to be the same for both motors
-    float Kp = 1;
-    float Ki = 0.8;
-    float Kd = 1;
+    float Kp = 64.9;
+    float Ki = 3.64;
+    float Kd = 5;
     
     //Proportional part:
     double u_k2 = Kp * error2;
@@ -197,36 +203,73 @@
     motor.attach(measureandcontrol, timeinterval); 
     while(true)
     {
-        wait(0.5);
-        //pc.printf("number of counts %i\r\n", counts);
-        //pc.printf("measured position is %f\r\n", measuredposition);
-        //pc.printf("motorvalue is %f\r\n", motorvalue);
-        //pc.printf("u_i is %d\r\n", u_i);
-        float potmeter1 = 5+pot1.read()*20;
-        float potmeter2 = pot2.read()*10;
+        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("x is %f\r\n",xendeffector);
-        pc.printf("y is %f\r\n",yendeffector);
-        //pc.printf("Kp is %f\r\n", Kp);
+        pc.printf("y is %f\r\n",yendeffector);        
         
-            
-    char a = pc.getcNb();
+        // Control position    
+        char a = pc.getcNb();
     
-    if(a == 'r'){
-        xendeffector=xendeffector+1;}
-        else if(a=='l'){
+        if(a == 'r' && xendeffector < 14){
+            xendeffector=xendeffector+1;}
+        else if(a=='l' && xendeffector > -14){
             xendeffector=xendeffector-1;}
-        else if(a=='u'){
+        else if(a=='u' && yendeffector < 30){
             yendeffector=yendeffector+1;}
-        else if(a=='d'){
+        else if(a=='d' && yendeffector > 5){
             yendeffector=yendeffector-1;}
+        
+        // Go back to starting position    
+        if (but1.read() == 0) { 
+            xendeffector=0; 
+            yendeffector=10.6159;
+        }
+        
+        if (but2.read() == 0){
+            xendeffector=-5; 
+            yendeffector=10.6159;
+            wait(0.5);
+            
+            xendeffector=10; 
+            yendeffector=25.6159;
+            wait(0.5);
+            
+            xendeffector=-14; 
+            yendeffector=21.6159;
+            wait(0.5);
+            
+            xendeffector=10; 
+            yendeffector=11.6159;
+            wait(0.5);
+            
+            xendeffector=0; 
+            yendeffector=10.6159;
+            wait(0.5);
+            }
+            
+        if (but3.read() ==0){
+           
+            xendeffector=-5;
+            yendeffector=10.6159;
+            int i=0;
+            wait(0.5);
+            while(i<100){
+                xendeffector=xendeffector+0.1;
+                yendeffector=yendeffector+0.1;
+                i++;
+                wait(0.001);
+            }
+        }
+            
+            
             
         
-        
+        // type c to stop the program
         char c = pc.getcNb();
-        
-        // type c to stop the program
         if (c == 'c')
         {
             pc.printf("Program stopped running");