Diff: main.cpp

Revision:

8:0b7925095416

Parent:

7:c17f5473f4e1

Child:

9:22d79a4a0324

--- a/main.cpp	Tue Oct 17 06:29:42 2017 +0000
+++ b/main.cpp	Fri Oct 20 14:38:14 2017 +0000
@@ -2,43 +2,85 @@
 #include "Motor.h"
 #include "HIDScope.h"
 #include "MODSERIAL.h"
-
-//Initialize Analog EMG inputs
-EMG EMG_bi_r(A0);
-EMG EMG_bi_l(A1);
-EMG EMG_tri_r(A2);
-EMG EMG_tri_l(A3);
-
-
-
-HIDScope scope(1); // 4 channels of data
+#include "iostream"
+DigitalIn a(D3);
+DigitalIn b(D4);
+HIDScope scope(2); // 4 channels of data
 Ticker MainTicker;
 MODSERIAL pc(USBTX, USBRX);
 
-const double sample_time= 0.002;   //fs = 500Hz
+/*****************************************************************/
+//Initialize Analog EMG inputs:
+
+
+EMG EMG_bi_r(A0);// Move  the endpoint to the right (plus direction)
+EMG EMG_bi_l(A1);// Move the endpoint to the left (minus direction)
+EMG EMG_tri_r(A2);// Move the endpoint forward (plus direction)
+EMG EMG_tri_l(A3);// Move the endpoint backward (minus direction)
+
+/****************************************************/
+
+Motor motor2;
 
 
+/*****************************************************/
+// Set control signals:
 
+double x_control_signal=0 , y_control_signal; //x direction is the righ/left movement
+
+
+double make_X_control_signal(){
+    double emg=EMG_bi_r.filter()/10;
+    double emg2=EMG_bi_l.filter()/10;
+    
+        if(abs(emg-emg2)>=0.0025)
+        {
+            return x_control_signal+emg2-emg;
+        }
+        else
+            return x_control_signal;
+    
+}
+
+/******************************************************/
+
+// Ticker Function:
 void ReadAndFilterEMG()
 {    
-    
+    //motor2.set_angle();
+    //x_control_signal= make_X_control_signal();
     
-    scope.set(0, EMG_bi_r.filter());
+    motor2.Control_angle(x_control_signal);
+    scope.set(0, x_control_signal);
+    scope.set(1, motor2.set_angle());
     scope.send();
-    /*****/ 
+    
 }
 
+
+/***************************************************/
+
+//Main Function:
 int main(void)
 {
+    
+    double sample_time= 0.002;   //fs = 500Hz
     pc.baud(115200);    //Set Baud rate for Serial communication
     MainTicker.attach(&ReadAndFilterEMG, sample_time);    //Attach time based interrupt
     
-    /*
+    
     while(true)
     {
-        
+        if(a==0){
+            x_control_signal+=10;
+            wait(0.1);
+            }
+        if(b==0){
+            x_control_signal-=10;
+            wait(0.1);
+            }
     }
-    */
+    
     
-    return 0;
+    //return 0;
 }
\ No newline at end of file