Diff: main.cpp

Revision:

8:5f13198a8e49

Parent:

7:87d9904c1c19

--- a/main.cpp	Tue Oct 27 10:49:59 2015 +0000
+++ b/main.cpp	Tue Oct 27 11:56:15 2015 +0000
@@ -2,6 +2,7 @@
 #include "HIDScope.h"
 #include "biquadFilter.h"        // Require the HIDScope library
 #include "MODSERIAL.h"
+#include "QEI.h"
 //Define objects
 AnalogIn    emg(A0); //Analog of EMG input
 Ticker      sample_timer;
@@ -11,7 +12,16 @@
 DigitalIn button1(PTA4);//test button for starting motor 1
 DigitalOut led1(LED_RED);
 DigitalOut led2(LED_BLUE);
+DigitalOut led3(LED_GREEN);
 MODSERIAL pc(USBTX,USBRX);
+// motor objects
+QEI motor1(D13,D12,NC, 624);//encoder for motor 1
+QEI motor2(D11,D10,NC, 624);//encoder for motor 2
+DigitalOut direction1(D7);//direction input for motor 1
+DigitalOut direction2(D4);//direction input for motor 2
+PwmOut speed1(D6);//speed input for motor 1
+PwmOut speed2(D5);//speed input for motor 2
+
 /*The biquad filters required to transform the EMG signal into an usable signal*/
 biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
 biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
@@ -26,6 +36,21 @@
 double onoffsignal;
 double maxcal=0;        
 bool calyes=0;
+bool motor1_dir=0;//set the direction of motor 1
+bool motor2_dir = 0;//set the direction of motor 1
+
+float cycle = 0.3;//define the speed of the motor
+   bool motor1_on = 1;//set the on variable of motor 1
+   bool motor2_on =1;//set the on variable of motor 2
+   int n1=1;//numeric conditions to determine if the speed needs to be increased
+   int n2=1;
+
+void changedirmotor1(){
+        motor1_dir = !motor1_dir;//code for changing direction of motor 1
+        }
+void changedirmotor2(){
+        motor2_dir = !motor2_dir;//code for changing direction of motor 2
+        }        
 
 /* 
  */
@@ -46,21 +71,34 @@
 }
 void checkmotor(){//check the normalized signal and set actions if a threshold is passed
     if(calyes==1){
-    if(onoffsignal<=0.25){
+    if(onoffsignal >= 0.5){
+                 led1.write(0);
+                 led2.write(1);
+            while(n1 == 1){
+                changedirmotor1();
+                speed1.write(cycle);//write speed only on first run through the loop
+                direction1.write(motor1_dir);//turn motor CCW or CW 
+                  
+                n1=0;
+                  }
+             }
+    else if(onoffsignal<=0.25){
              led1.write(1);
              led2.write(0);
+             
+            while(n1==0){//check if the first run was done
+                 speed1.write(0);//if so set speed to 0 and reset the run counter
+                 n1=1;
              }
-    else if(onoffsignal >= 0.5){
-                 led1.write(0);
-                 led2.write(1);
-             }
-             }
+            }
+    
+        }
 }
 
 void calibri(){//calibration function
     if(button1.read()==false){
         for(int n =0; n<5000;n++){//read for 5000 samples as calibration
-            emg_value = emg.read();//read the emg value from the elektrodes
+            emg_value = emg.read();//read the emg value from the electrodes
             signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
             signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
             signalpart3 = abs(signalpart2);//low pass filter to envelope the emg