Revision 4:e59a99c5aa08, committed 2016-10-25

Comitter:

NahuelM

Date:

Tue Oct 25 10:36:50 2016 +0000

Parent:

3:1d43dd4f37eb

Commit message:

Servo controller ge?mplementeert;

Changed in this revision

--- a/main.cpp	Mon Oct 24 11:43:45 2016 +0000
+++ b/main.cpp	Tue Oct 25 10:36:50 2016 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "QEI.h"
 
+#include "servoController.h"
 #include "emg.h"
 
 #define pi 3.14159265359;
@@ -13,12 +14,12 @@
 AnalogIn Potmeter2(A1);
 QEI Slide_Encoder(D12,D13,NC,64);
 QEI Lift_Encoder(D10,D11,NC,64);
-Serial pc(USBTX,USBRX);
+//Serial pc(USBTX,USBRX);
 Ticker Controller;
 
-char Key;
+//char Key;
 bool Controller_Flag=0;
-float Frequency = 30;
+//float Frequency = 30;
 float Frequency_PWM = 10000;
 
 float Slide_Radius = 12.5;
@@ -86,6 +87,10 @@
     change_state2.attach( &run,10);
     emgSampleTicker.attach( &emgSample, 0.002);
     
+    treshold = (cali_max-cali_min)*treshold_multiplier;
+    servoTick.attach(&control_servo, 1/Frequency);
+    ServoPWMpin.period(0.01f);                          // 0.01 second period
+    
     while (true) {
         pc.printf("\n\r%f", Norm_EMG_0);
          
@@ -96,6 +101,8 @@
         if (Controller_Flag == true){
             Slide_Controller();
             Lift_Controller();
+            control_servo();
+            
             Controller_Flag = false;
         }
         

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servoController.h	Tue Oct 25 10:36:50 2016 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+
+Serial pc(USBTX,USBRX);
+
+PwmOut ServoPWMpin(D8);
+Ticker servoTick;
+float i = 0;
+char Key;
+double ServoAngle = 89 ;
+float Pulsew = 0.0015;
+const float Frequency = 10;
+double input_signal = 0;
+double cali_min = 0;
+double cali_max = 1;
+double treshold = 0.5;
+float treshold_multiplier = 0.5;
+bool binary_input_signal = 0;
+bool binary_input_signal_previous = 0;
+
+void control_servo(){
+    if (input_signal > treshold){                        // convert the emg to a zero or a one
+        binary_input_signal = 1;
+    } else {
+        binary_input_signal = 0;
+    }
+    if(( binary_input_signal!= binary_input_signal_previous)&& binary_input_signal){
+        if( ServoAngle < 45){                           // check wether it is more opened or closed
+            ServoAngle = 89;                            // open 
+        }
+        else{
+            ServoAngle = 1;                             // close 
+        }
+    }
+    Pulsew = 0.0015+(ServoAngle)/180000;                // calculate the pulsewidth in the range 1 to 2 milliseconds
+    ServoPWMpin.pulsewidth(Pulsew);                     // write the pulsewidth
+    pc.printf("\n\r Pulsew is %f",Pulsew);
+    
+    binary_input_signal_previous = binary_input_signal;
+}
+/*
+int main(){
+    pc.printf("\n\r ----------------------------------------\n\r --------------- START -----------------\n\r ----------------------------------------");
+    treshold = (cali_max-cali_min)*treshold_multiplier;
+    servoTick.attach(&control_servo, 1/Frequency);
+    ServoPWMpin.period(0.01f);                          // 0.01 second period
+    
+    while (true) {        
+        Key = pc.getc();                            // get the pressed key
+        switch(Key) {                               //Check to see which key pressed
+            case 0x2B:                              //It was the + key...
+                pc.printf("\n\r +!");
+                if( ServoAngle < 80){
+                    ServoAngle = ServoAngle+10;     // increase the angle
+                }
+                break;
+            case 0x2D:                              //It was the - Key key...
+                pc.printf("\n\r -!");
+                if( ServoAngle > 10){
+                    ServoAngle = ServoAngle-10;     // decrease the angle
+                }
+                break;
+            case 0x20:                              //It was the Spacebar key...
+                pc.printf("\n\r SPACE!");
+                if( ServoAngle < 45){               // Switch from open to closed or else otherwise
+                    ServoAngle = 89;                // open/close 
+                }
+                else{
+                    ServoAngle = 1;                 // open/close 
+                }
+                break;
+            }
+
+    }
+}
+*/
\ No newline at end of file