Diff: main.cpp

Revision:

2:44758d95cb0b

Parent:

0:a6f2b6cc83ca

Child:

3:4b6b3b5e3a1b

Child:

4:85770b268599

--- a/main.cpp	Tue Oct 30 16:04:17 2018 +0000
+++ b/main.cpp	Wed Oct 31 15:08:58 2018 +0000
@@ -18,6 +18,7 @@
 
 // PID  CONTROLLER     --        PIN DEFENITIONS 
 AnalogIn button2(A4);
+AnalogIn button1(A3);
 DigitalOut directionpin1(D7);   // motor 1
 PwmOut pwmpin1(D6);             // motor 1
 
@@ -34,9 +35,9 @@
 
 // CONSTANTS PID CONTROLLER
 double PI = M_PI;// CHANGE THIS INTO M_PI
-double Kp = 16; //200 , 50
+double Kp = 14; //200 , 50
 double Ki = 0;   //1, 0.5
-double Kd = 4.5; //200, 10
+double Kd = 3; //200, 10
 double Ts = 0.1; // Sample time in seconds
 double reference_rotation; //define as radians
 double motor_position;
@@ -219,7 +220,7 @@
 void Motor_mover()
 {
     double motor_position = encoder1.getPulses(); //output in counts
-    double reference_rotation = hoek3(px, py);
+    double reference_rotation = hoek2(px, py);
     double error = reference_rotation - motor_position*(2*PI)/8400;
     double u = PID_controller(error);
     moter_control(u);
@@ -256,11 +257,27 @@
         Scope_Data.attach(ScopeData, 0.01);
         
 
-// berekenen positie
-   float px = positionx(1,0);  // EMG: +x, -x
-   float py = positiony(1,0);  // EMG: +y, -y
-   //printf("positie (%f,%f)\n\r",px,py);
+while(true){ 
+ 
+        
+if (button2 == false)
+{
+   wait(0.05f);
+        
+        // berekenen positie
+           float px = positionx(1,0);  // EMG: +x, -x
+            float py = positiony(1,0);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+                }        
 
+if (button1 == false){
+    wait(0.05f);
+            // berekenen positie
+           float px = positionx(0,1);  // 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);