servoController.h

00001 #include "mbed.h"
00002 
00003 Serial pc(USBTX,USBRX);
00004 PwmOut ServoPWMpin(D9);
00005 Timeout resetter;
00006 
00007 bool readtime = 1;
00008 double ServoAngle = 89 ;
00009 float Pulsew = 0.0015;
00010 double treshold = 0.3;
00011 bool binary_input_signal = 0;
00012 bool binary_input_signal_previous = 0;
00013 
00014 void set21(){
00015     readtime = 1;
00016 }
00017 
00018 void control_servo(double input_signal){
00019     //pc.printf("/n/r %f", input_signal);
00020     if (input_signal > treshold){                        // convert the emg to a zero or a one
00021         binary_input_signal = 1;
00022     } else {
00023         binary_input_signal = 0;
00024     }
00025     if((( binary_input_signal!= binary_input_signal_previous)&& binary_input_signal)&& readtime){
00026         readtime = 0;
00027         resetter.attach(&set21, 1);
00028         if( ServoAngle < 45){                           // check wether it is more opened or closed
00029             ServoAngle = 89;                            // open 
00030         }
00031         else{
00032             ServoAngle = 1;                             // close 
00033         }
00034     }
00035     Pulsew = 0.0015+(ServoAngle)/180000;                // calculate the pulsewidth in the range 1 to 2 milliseconds
00036     ServoPWMpin.pulsewidth(Pulsew);                     // write the pulsewidth
00037     //pc.printf("\n\r Pulsew is %f",Pulsew);
00038     
00039     binary_input_signal_previous = binary_input_signal;
00040 }