Diff: main.cpp

Revision:

2:dc9766657afb

Parent:

1:4bf64d003f3a

Child:

3:c8f0fc045505

--- a/main.cpp	Mon Oct 15 13:55:45 2018 +0000
+++ b/main.cpp	Mon Oct 22 08:15:41 2018 +0000
@@ -1,5 +1,8 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "HIDScope.h"
+#include "QEI.h"
+
 MODSERIAL pc(USBTX, USBRX);
 DigitalOut DirectionPin1(D4);
 DigitalOut DirectionPin2(D7);
@@ -8,15 +11,46 @@
 DigitalIn Knop1(D2);
 AnalogIn pot1 (A5);
 AnalogIn pot2 (A4);
+AnalogIn emg0( A0 );
+AnalogIn emg1( A1 );
+AnalogIn emg2( A2 );
+AnalogIn emg3( A3 );
 
-Ticker mycontrollerTicker1;
-Ticker mycontrollerTicker2;
-Ticker Velo1;
-Ticker Velo2;
+Ticker      printTicker;
+Ticker      mycontrollerTicker1;
+Ticker      mycontrollerTicker2;
+Ticker      Velo1;
+Ticker      Velo2;
+Ticker      EMG_Read_Ticker;
+Ticker      sample_timer;
+HIDScope    scope( 4 );
 
-//const float maxVelocity=8.4; // in rad/s
+volatile float Bicep_Right = 0.0;
+volatile const float maxVelocity=8.4; // in rad/s
 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
 volatile float referenceVelocity2 = 0.5;
+
+volatile int counts1;
+volatile int counts2;
+QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
+QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
+
+void EMG_Read()
+{
+    Bicep_Right = emg0.read();
+}
+
+void sample()
+{
+    
+    scope.set(0, emg0.read() );
+    scope.set(1, emg1.read() );
+    scope.set(2, emg2.read() );
+    scope.set(3, emg3.read() );
+  
+    scope.send();
+}
+
    
 void velocity1()
     {
@@ -72,15 +106,37 @@
         PwmPin2 = fabs(u);
     }
 
+void Printing()
+{
+    float v1 = fabs(referenceVelocity1) * maxVelocity;
+    float v2 = fabs(referenceVelocity2) * maxVelocity;
+    
+    //eventueel nog counts -> rad/s 
+    
+    //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
+    pc.printf("%i counts van m1, %i counts van m2", counts1, counts2);
+    
+    
+}
+
 int main()
 {
     pc.baud(115200);    
     PwmPin1.period_us(120); //60 microseconds pwm period, 16.7 kHz 
+    
+    counts1 = Encoder1.getPulses();
+    counts2 = Encoder2.getPulses();
+    
+    sample_timer.attach(&sample, 0.002);
+    EMG_Read_Ticker.attach(&EMG_Read, 0.002);
+    
     mycontrollerTicker1.attach(motor1, 0.002);//500Hz
     Velo1.attach(velocity1, 0.002);
     mycontrollerTicker2.attach(motor2, 0.002);
     Velo2.attach(velocity2, 0.002);
     
+    printTicker.attach(&Printing, 2.0);
+    
     while(true)
     {        
     }