Diff: main.cpp

Revision:

4:bf7765b0f612

Parent:

3:48438eea184e

Child:

5:8ac5d0651e4d

--- a/main.cpp	Tue Oct 13 13:42:16 2015 +0000
+++ b/main.cpp	Tue Oct 13 14:22:01 2015 +0000
@@ -11,7 +11,7 @@
 Serial pc(USBTX,USBRX);
 
 ////////////////ENCODERS
-QEI encoder1(D12,D13,NC,32);
+QEI encoder1(D12,D13,NC,32);/// maybe use Encoder in stead of QEI, because Encoder had setposition
 QEI encoder2(D10,D11,NC,32);
 
 //////////////////////////////////CONTROLLER
@@ -37,7 +37,7 @@
 Ticker safetyandthreshold_ticker; // ticker to read potmeters
 const double safetyandthreshold_frequency=1; // frequency for the ticker
 
-float threshold_value=1;
+float threshold_value=1;//initial threshold value
 
 ////////////////////////////////FILTER
 #include "filtervalues.h"
@@ -57,7 +57,12 @@
 Ticker readsignal_ticker;
 const double readsignal_frequency=25;
 
-DigitalOut led(D4);
+DigitalOut led1(D8);
+DigitalOut led2(D9);
+
+//////////////////////////////// POSITION AND ANGLE SHIZZLE
+float desired_position=0;
+float desired_angle[]= {0,0};
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
@@ -95,7 +100,7 @@
     scope.set(0,emg1_input.read());
     scope.set(1,filteredsignal1);
     scope.send();
-    }
+}
 //read potmeters and adjust the safetyfactor and threshold
 void safetyandthreshold()
 {
@@ -169,11 +174,18 @@
 }
 void readsignal()
 {
-    if (filteredsignal1>=threshold_value) {
-        led=1;
+    if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
+        led1=led2=1;
+    } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
+        led1=1;
+        led2=0;
+    } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
+        led1=0;
+        led2=1;
     } else {
-        led=0;
+        led1=led2=0;
     }
+
 }