Diff: main.cpp

Revision:

6:1f722bf6a89b

Parent:

4:5ceb8f058874

Child:

7:b53f0c4cf2b9

--- a/main.cpp	Wed Oct 24 11:42:35 2018 +0000
+++ b/main.cpp	Fri Oct 26 08:53:26 2018 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include "math.h"
 #include "BiQuad.h"
-//#include "Servo.h"
-#include "HIDScope.h"
+#include "Servo.h"
+
 
 
 
@@ -15,12 +15,18 @@
 DigitalOut  ledG(LED_GREEN);                                                    // Informative LED for gaining zero and max
 DigitalIn   CalButton(PTA4);                                                    // Button used for gaining zero and max
 DigitalIn   zeromax(PTC6);                                                      // Button used for switching between zero and max
-Ticker      emgSampleTicker;                                                    // Ticker for sample frequency
-HIDScope    scope( 6 );                                                       
-//Servo myservo(D13);
+Ticker      emgSampleTicker;                                                    // Ticker for sample frequency           
+                                          
+Servo myservo(D13);
+
 DigitalOut motor1direction(D7);
 PwmOut motor1control(D6);
 
+DigitalOut motor2direction(D4);
+PwmOut motor2control(D5);
+
+InterruptIn button1(D10);
+InterruptIn button2(D11);
 
  int P= 200;                                                                    // Number of points for movag first emg
  int Q = 200;                                                                   // Number of points for movag second emg
@@ -43,6 +49,11 @@
  float thresholdR = 10;                                                         // - before the zero and max values are calculated
  float thresholdS = 10;                                                         //-------
  
+const bool clockwise1 = true;                                                    //Clockwise direction for motor 1
+const bool clockwise2 = true;                                                   //Clockwise direction for motor 2
+volatile bool direction1 = clockwise1;                                            //Bool to make direction 1 clockwise
+volatile bool direction2 = clockwise2;                                          //Bool to make direction 2 clockwise
+ 
  //------------Filter parameters----------------------
 
 //Lowpassfilter
@@ -149,16 +160,16 @@
     movagS = sumS/R;     
     
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
-    scope.set(0, emgL.read());
-    scope.set(1, movagL);
-    scope.set(2, emgR.read());
-    scope.set(3, movagR);
-    scope.set(4, emgS.read());
-    scope.set(5, movagS);
+    //scope.set(0, emgL.read());
+//    scope.set(1, movagL);
+//    scope.set(2, emgR.read());
+//    scope.set(3, movagR);
+//    scope.set(4, emgS.read());
+//    scope.set(5, movagS);
     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
-    scope.send();                                                     // Moving average value left biceps
+  //  scope.send();                                                     // Moving average value left biceps
 
 
     if (CalButton==0 & k<200) {                                                 // Loop used for gaining max and zero value
@@ -208,6 +219,18 @@
         thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
     }
 }
+//----------------Buttons to change direction motors----------------------------
+void onButtonPress1() {
+    // reverses the direction
+    motor1direction.write(direction1 = !direction1);
+    //pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
+}
+
+void onButtonPress2() {
+    // reverses the direction
+    motor2direction.write(direction2 = !direction2);
+    //pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
+}
 
 int main()
 {  
@@ -218,23 +241,36 @@
 Lwaarde[0] = 0.01;                                                              // Startvalue for zerovalue, to - 
 Rwaarde[0] = 0.01;                                                              // - prevent dividing by 0
 Swaarde[0] = 0.01;
+
+    button1.fall(&onButtonPress1);
+    button2.fall(&onButtonPress2);
+    
     while(1) {
         
-     //   if (movmeanR > thresholdR)
-//        {   myservo = 0.5;
-//            wait(0.01);
-//        }
-//        else {
-//            myservo = 0.0;
-//            wait(0.01);
-//            }
-//            
-//        if (movmeanL > thresholdL)
-//        {   motor1control.write(0.8);
-//            motor1direction.write(true);
-//            }
-//        else {
-//            motor1control.write(0);
-//            }
+            
+            
+        if (movagL > thresholdL)
+        {   motor1control.write(0.8);
+            }
+        else {
+            motor1control.write(0);
+            }
+            
+        if (movagR > thresholdR)
+        {   motor2control.write(0.8);
+            }
+        else {
+            motor2control.write(0);
+            }
+            
+            
+        if (movagS > thresholdS)
+        {   myservo = 0.5;
+            wait(0.01);
+        }
+        else {
+            myservo = 0.0;
+            wait(0.01);
+            }
     }
 }
\ No newline at end of file