Diff: main.cpp

Revision:

6:37c94a5e205f

Parent:

5:8ac5d0651e4d

Child:

7:7fbb2c028778

--- a/main.cpp	Wed Oct 14 12:29:27 2015 +0000
+++ b/main.cpp	Wed Oct 14 13:00:50 2015 +0000
@@ -14,6 +14,10 @@
 QEI encoder1(D12,D13,NC,32);/// maybe use Encoder in stead of QEI, because Encoder had setposition
 QEI encoder2(D10,D11,NC,32);
 
+/////////////////////////////////CALIBRATION (MODE)
+int modecounter=0;
+DigitalIn changemodebutton(PTA4);
+
 //////////////////////////////////CONTROLLER
 controlandadjust mycontroller; // make a controller
 //controller constants
@@ -69,7 +73,8 @@
               control_go=false,
               filter_go=false,
               safetyandthreshold_go=false,
-              readsignal_go=false;
+              readsignal_go=false,
+              switchedmode=false;
 
 void scopedata_activate()
 {
@@ -188,6 +193,17 @@
 
 }
 
+void changemode()
+{
+    switchedmode=true;
+    modecounter++;
+    if (modecounter==3) {
+        modecounter=0;
+    } else {
+        modecounter=modecounter;
+    }
+    wait(1);
+}
 
 int main()
 {
@@ -199,26 +215,52 @@
     readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
 
     while(1) {
-        if (scopedata_go==true) {
-            scopedata();
-            scopedata_go=false;
-        }
-        if (filter_go==true) {
-            filtereverything();
-            filter_go=false;
+        if (changemodebutton==0) {
+            changemode();
         }
-        if (safetyandthreshold_go==true) {
-            safetyandthreshold();
-            safetyandthreshold_go=false;
+        if(modecounter==0) {//normal running mode
+            if (switchedmode==true) {
+                encoder1.reset();
+                encoder2.reset();
+                pc.printf("Program running\n");//
+                switchedmode=false;
+            }
+            if (scopedata_go==true) {
+                scopedata();
+                scopedata_go=false;
+            }
+            if (filter_go==true) {
+                filtereverything();
+                filter_go=false;
+            }
+            if (safetyandthreshold_go==true) {
+                safetyandthreshold();
+                safetyandthreshold_go=false;
+            }
+            if (control_go==true) {
+                control();
+                control_go=false;
+            }
+            if (readsignal_go==true) {
+                readsignal();
+                readsignal_go=false;
+            }
+            valuechangebutton.fall(&valuechange);
         }
-        if (control_go==true) {
-            control();
-            control_go=false;
+        if (modecounter==1) {
+            if(switchedmode==true) {
+                pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
+                switchedmode=false;
+            }
+
         }
-        if (readsignal_go==true) {
-            readsignal();
-            readsignal_go=false;
+
+        if (modecounter==2) {
+            if(switchedmode==true) {
+                pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
+                switchedmode=false;
+            }
+
         }
-        valuechangebutton.fall(&valuechange);
     }
 }
\ No newline at end of file