Diff: main.cpp

Revision:

9:59dc4c12e4ee

Parent:

8:cfe7ad86837c

--- a/main.cpp	Wed Oct 31 23:05:26 2018 +0000
+++ b/main.cpp	Thu Nov 01 15:37:21 2018 +0000
@@ -19,6 +19,8 @@
 // PID  CONTROLLER     --        PIN DEFENITIONS 
 AnalogIn button2(A4);
 AnalogIn button1(A3);
+DigitalIn button3(SW2);
+DigitalIn button4(SW3);
 
 DigitalOut directionpin1(D7);   // motor 1
 DigitalOut directionpin2(D4);   // motor 2
@@ -91,10 +93,10 @@
 float da3 = 3.372859;
 
 // limits (since no forward kinematics)
-float upperxlim = 0.25; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
-float lowerxlim = 0.15;
-float upperylim = 0.20;
-float lowerylim = 0.10;
+float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
+float lowerxlim = 0.10;
+float upperylim = 0.225;
+float lowerylim = 0.03; //0.03 is goed
 
 
 //----------------FUNCTIONS--------------------------
@@ -224,19 +226,38 @@
 // DIRECTON AND SPEED CONTROL
 void moter_control(double u)
 {
+    
     directionpin1= u > 0.0f; //eithertrueor false
+    if (fabs(u)> 0.7f){
+        u = 0.7f;
+        }
+    else {
+        u= u;
+        }      
     pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 void moter2_control(double u)
 {
     directionpin2= u > 0.0f; //eithertrueor false
+        if (fabs(u)> 0.7f){
+        u = 0.7f;
+        }
+    else {
+        u= u;
+        }  
     pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 void moter3_control(double u)
 {
     directionpin3= u > 0.0f; //eithertrueor false
+    if (fabs(u)> 0.7f){
+        u = 0.7f;
+        }
+    else {
+        u= u;
+        }  
     pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
@@ -304,7 +325,7 @@
         
         // berekenen positie
            float px = positionx(1,0);  // EMG: +x, -x
-            float py = positiony(1,0);  // EMG: +y, -y
+            float py = positiony(0,0);  // EMG: +y, -y
             //printf("positie (%f,%f)\n\r",px,py);
                 }        
 
@@ -312,10 +333,29 @@
     wait(0.01f);
             // berekenen positie
            float px = positionx(0,1);  // EMG: +x, -x
+            float py = positiony(0,0);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+            }
+
+if (button3 == false){
+    wait(0.01f);
+            // berekenen positie
+           float px = positionx(0,0);  // EMG: +x, -x
+            float py = positiony(1,0);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+            }
+
+if (button4 == false){
+    wait(0.01f);
+            // berekenen positie
+           float px = positionx(0,0);  // EMG: +x, -x
             float py = positiony(0,1);  // EMG: +y, -y
             //printf("positie (%f,%f)\n\r",px,py);
             }
+
 }
+
+
 // berekenen hoeken
 /*
 float a1 = hoek1(px, py);