Diff: main.cpp

Revision:

5:e7253884c2e4

Parent:

4:5ceb8f058874

--- a/main.cpp	Wed Oct 24 11:42:35 2018 +0000
+++ b/main.cpp	Wed Oct 24 12:09:27 2018 +0000
@@ -1,8 +1,9 @@
 #include "mbed.h"
 #include "math.h"
 #include "BiQuad.h"
-//#include "Servo.h"
-#include "HIDScope.h"
+#include "Servo.h"
+#include "QEI.h"
+//#include "HIDScope.h"
 
 
 
@@ -16,10 +17,20 @@
 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);
+//HIDScope    scope( 6 );                                                       
+Servo myservo(D8);
 DigitalOut motor1direction(D7);
 PwmOut motor1control(D6);
+DigitalOut motor2direction(D4);
+PwmOut motor2control(D5);
+InterruptIn button1(D10);
+InterruptIn button2(D11);
+
+QEI EncoderL(D12,D13,NC,64,QEI::X2_ENCODING);
+QEI EncoderR(D2,D3,NC,64,QEI::X2_ENCODING);
+
+Ticker PrintTicker;
+Serial pc(USBTX, USBRX);
 
 
  int P= 200;                                                                    // Number of points for movag first emg
@@ -42,6 +53,10 @@
  float thresholdL = 10;                                                         // Startvalue for threshold, to block signals -
  float thresholdR = 10;                                                         // - before the zero and max values are calculated
  float thresholdS = 10;                                                         //-------
+ const bool clockwise1 = true;
+ const bool clockwise2 = true;
+ volatile bool direction1 = clockwise1;
+ volatile bool direction2 = clockwise2;
  
  //------------Filter parameters----------------------
 
@@ -149,16 +164,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
@@ -206,11 +221,56 @@
         thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
         thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
         thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
-    }
+    }   
+}
+
+
+//--------------------------Directions motor 1 aanpassen-------------------------
+void onButtonPress1() {
+    // reverses the direction
+    motor1direction.write(direction1= !direction1);
+    pc.printf("direction1: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
+}
+
+//--------------------------Directions motor 2 aanpassen-------------------------
+void onButtonPress2() {
+    // reverses the direction
+    motor2direction.write(direction2= !direction2);
+    pc.printf("direction2: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
+}
+
+//-----------------------Print functie----------------------------------------------
+void Printen() {
+    
+    const float pi = 3.141592653589793;                                             // Value of pi
+    double gearratio = 3.857142857;
+    double radiuspulley = 15.915;
+    double hoekgraad = EncoderL.getPulses() * 0.0857142857;
+    double hoekrad = hoekgraad * 0.0174532925;
+    double hoekgraad2 = EncoderR.getPulses() * 0.0857142857;
+    double hoekrad2 = hoekgraad2 * 0.0174532925;
+    double hoekarm = hoekgraad2 / gearratio;
+    double translatie = hoekgraad /360 * 2 * pi * radiuspulley;
+    
+
+    pc.printf("counts :%i \r\n", EncoderL.getPulses());
+    pc.printf("hoekgraad :%f \r\n", hoekgraad );
+    pc.printf("hoekrad :%f \r\n", hoekrad );
+    pc.printf("translatie :%f mm \r\n", translatie );
+    pc.printf("\n");
+    
+    
+    pc.printf("counts2 :%i \r\n", EncoderR.getPulses());
+    pc.printf("hoekgraad2 :%f \r\n", hoekgraad2 );
+    pc.printf("hoekrad2 :%f \r\n", hoekrad2 );
+    pc.printf("hoekarm :%f \r\n", hoekarm );
+    pc.printf("\n");
+    
 }
 
 int main()
 {  
+//------------------EMG samplen-----------------------------
 ledB = 1;
 ledG = 1;
 ledR = 1;
@@ -218,23 +278,44 @@
 Lwaarde[0] = 0.01;                                                              // Startvalue for zerovalue, to - 
 Rwaarde[0] = 0.01;                                                              // - prevent dividing by 0
 Swaarde[0] = 0.01;
+
+//-----------------Motor button directions----------------------------
+pc.baud(115200);
+    
+    button1.fall(&onButtonPress1);
+    button2.fall(&onButtonPress2);
+    PrintTicker.attach(&Printen, 1);
+
+
+//------------------Oneindige loop--------------------------------------
     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);
+            motor1direction.write(true);
+            }
+        else {
+            motor1control.write(0);
+        }
+            
+        if (movagR > thresholdR)
+        {   motor2control.write(0.8);
+            motor2direction.write(true);
+            }
+        else {
+            motor1control.write(0);
+        }
+            
+            
+        if (movagS > thresholdS)
+        {   myservo = 0.5;
+            wait(0.01);
+        }
+        else {
+            myservo = 0.0;
+            wait(0.01);
+        }
+            
+
     }
 }
\ No newline at end of file