Diff: NR_method_1.cpp

Revision:

2:f68fd7b1c655

Parent:

1:fafea1d00d0c

Child:

3:40427c0157a0

--- a/NR_method_1.cpp	Thu Nov 01 09:42:53 2018 +0000
+++ b/NR_method_1.cpp	Thu Nov 01 11:13:32 2018 +0000
@@ -10,7 +10,8 @@
 //#include  <MODSERIAL.h>
 
 bool bas;
-bool printen;
+int waiting;
+InterruptIn button(SW3);
 
 Serial pc(USBTX, USBRX);
 // emg signals input
@@ -78,7 +79,7 @@
 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
 
 double Kp = 1;
-double Ki = 0.6;
+double Ki = 1;
 double Kd = 0.3;
 double Ts = 0.001;
 
@@ -302,21 +303,59 @@
     }
 }
 
+
+void change_wait()
+{
+    waiting = waiting++;
+}
+
+
+void initializeren()
+{
+    waiting = 1;
+    while(waiting <=3)
+    if (bas == true)
+    {
+        if (waiting == 1) {
+            Cxx = -20;
+            Cyy = 10;
+
+            position_define();
+            angle_define();
+            motor_controler();
+
+        }
+
+    if(waiting == 2) {
+        Cxx = -40;
+        Cyy = 10;
+        position_define();
+        angle_define();
+        motor_controler();
+        button.fall(&change_wait);
+    }
+    }
+}
+
+
 int main()
 {
     PMW1.period_us(60);
+    button.fall(&change_wait);
+    position_controll.attach(position_controll_void,0.002);
     X[0][0] = X0[0][0];
     X[1][0] = X0[1][0];
     Xold[0][0] = X0[0][0];
     Xold[1][0] = X0[1][0];
-    pc.baud(115200);
+    //pc.baud(115200);
+    initializeren();
     x1 = Cxx;
     y1= Cyy;
     bqc.add( &bq1 ).add( &bq2 );
     bqc2.add( &bq3 );
     bqc3.add( &bq4 ).add( &bq5 );
     bqc4.add( &bq6 );
-    position_controll.attach(position_controll_void,0.001);
+
     while(true) {
 
         if(bas == true) {
@@ -325,7 +364,7 @@
             position_define();
             angle_define();
             motor_controler();
-            
+
             sw2.fall(change);
             Position1x(emgFiltered3);
             Position1y(emgFiltered23);