Galileo Hand Basic Example implemented on FRDM K64F

Dependencies:   NOKIA_5110 mbed-dsp mbed

Fork of Nucleo_EMG_Galileo_Hand by Julio Fajardo

main.cpp

Committer:
julioefajardo
Date:
2015-11-01
Revision:
6:78494092a326
Parent:
5:49c5553b6e2c

File content as of revision 6:78494092a326:

#include "mbed.h"
#include "arm_math.h" 

#define TRUE        1
#define FALSE       0
#define RAW         0
#define RECTIFIED   1
#define SMOOTH      2        
#define THRESHOLDF  0.25f
#define THRESHOLDE  0.25f
#define FLEXION2    0.0f

Ticker EMG_Sampler;
//Serial pc(SERIAL_TX, SERIAL_RX);
Serial pc(USBTX, USBRX);
DigitalOut myled(LED1);
InterruptIn sw2(SW2);
AnalogIn   Ref(A0);
AnalogIn   E1(A1);
AnalogIn   E2(A2);
AnalogIn   E3(A3);
PwmOut Thumb(D11);
PwmOut Index(D10);
PwmOut Middle(D9);
PwmOut Pinky(D6);
PwmOut ThumbRot(D5);
 
//EMG samples and DSP variables
float32_t EMG1, EMG2, EMG3;
float32_t samples[25];
float32_t samples2[25];
float32_t samples3[25];
float32_t abs_output[25];
float32_t abs_output2[25];
float32_t abs_output3[25];
float32_t mean = 0.0f;
float32_t mean2 = 0.0f; 
float32_t mean3 = 0.0f;

//variables for state machines
uint8_t state = 0;
uint8_t action = '0';

//conversion and data collection complete flag
uint8_t COCO = 0;

//functions declaration
void PWM_Init(void);
void ADC_Sampler(void);
void RX_Interrupt(void);
void Serial_Oscilloscope(uint8_t activation, uint8_t type);
void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us);

void flip() {
    if(action<='5') action++;
    else action = '1';
} 

int main() {
  pc.baud(115200);                                  //Serial com at 115200 bauds
  pc.attach(&RX_Interrupt);                         //Serial RX interrupt attachment
  sw2.rise(&flip); 
  EMG_Sampler.attach(&ADC_Sampler, 0.001);          //1 ms ticker for ADC Sampler
  PWM_Init();                                       //Servo initialization
  myled = 1;
  
  while(1) { 
    if(COCO){
        arm_abs_f32(samples, abs_output, 25);       //rectifier EMG1
        arm_abs_f32(samples2, abs_output2, 25);     //rectifier EMG2
        arm_abs_f32(samples3, abs_output3, 25);     //rectifier EMG3
        arm_mean_f32(abs_output, 25, &mean);        //mean EMG1
        arm_mean_f32(abs_output2, 25, &mean2);      //mean EMG2 
        arm_mean_f32(abs_output3, 25, &mean3);      //mean EMG3
        Serial_Oscilloscope(TRUE,SMOOTH);
        switch(state){   
            case 0: {
                if (mean>THRESHOLDF){
                    myled = 0;
                    state = 1;
                    switch(action){
                        case '1': FingerPosition(2400, 600, 600,2400,2400); break;  //Close
                        case '2': FingerPosition(2400,2400, 600,2400,2400); break;  //Point
                        case '3': FingerPosition(2400, 600,2400, 600,2400); break;  //Pinch
                        case '4': FingerPosition(2400, 600, 600,2400, 600); break;  //Lateral
                        case '5': FingerPosition(2400, 600, 600, 600,2400); break;  //Tripod
                        default:  FingerPosition(2400, 600, 600,2400,2400);         //Close
                    }
                }    
            } break;
            case 1: {
                if (mean2>THRESHOLDE){
                    myled = 1;
                    state = 0;
                    FingerPosition(1000,2400,2400, 600, 2400);                       //Open
                }       
            }
        }
        COCO = 0;   
    }   
  }
}

//EMG sampler Ts = 1ms
void ADC_Sampler() {
    EMG1 = (E1.read()-Ref.read())*3.3f;
    EMG2 = (E2.read()-Ref.read())*3.3f;
    EMG3 = (E3.read()-Ref.read())*3.3f;
    Serial_Oscilloscope(FALSE,RAW);
    uint32_t m = __get_PRIMASK();
    __disable_irq();        
    for(int j=24;j>0;j--) {
        samples[j]=samples[j-1];                    //Fill Array
        samples2[j]=samples2[j-1];                  //Fill Array
        samples3[j]=samples3[j-1];                  //Fill Array
    }
    samples[0]=EMG1;
    samples2[0]=EMG2;
    samples3[0]=EMG3;       
    __set_PRIMASK(m);
    COCO = 1;
}

//action selection trough serial console
void RX_Interrupt(void){
    action = pc.getc();
    /*switch(action){
        case '1': pc.printf("Power Grip\n"); break;
        case '2': pc.printf("Point\n"); break;
        case '3': pc.printf("Pinch\n"); break;
        case '4': pc.printf("Lateral\n"); break;
        case '5': pc.printf("Tripod\n"); break;
        default:  pc.printf("Power Grip\n");
    }*/
}

//PWM initialization for servos
void PWM_Init(void){
    //Open -> 0.6ms - Close 2.4ms
    Thumb.period(0.02f);
    Thumb.pulsewidth(0.0010f);
    //Open -> 2.4ms - Close 0.6ms
    //Index.period(0.02f);
    Index.pulsewidth(0.0024f);
    //Open -> 2.4ms - Close 0.6ms
    Middle.period(0.02f);
    Middle.pulsewidth(0.0024f);
    //Open -> 0.6ms - Close 2.4ms
    //Pinky.period(0.02f);
    Pinky.pulsewidth(0.0006f);
    //Open -> 0.6ms - Close 2.4ms
    ThumbRot.pulsewidth(0.0006f);  
}

//send data through serail port for oscilloscope visualization
void Serial_Oscilloscope(uint8_t activation, uint8_t type){
    if (activation){
        switch(type){
            case RAW: pc.printf("%.10f,%.10f\n\r",EMG1,EMG2); break; 
            case RECTIFIED: pc.printf("%.10f,%.10f\n\r",abs_output[0],abs_output2[0]);break;
            case SMOOTH: pc.printf("%.10f,%.10f\n\r",mean,mean2); break;
            default: pc.printf("%.10f,%.10f\n\r",EMG1,EMG2); 
        } 
    }    
}

//finger position through servos - values on microseconds (600 us - 2400 us)
void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us){
    Thumb.pulsewidth(thumb_us/1000000.0f);
    Index.pulsewidth(index_us/1000000.0f);
    Middle.pulsewidth(middle_us/1000000.0f);
    Pinky.pulsewidth(pinky_us/1000000.0f);
    ThumbRot.pulsewidth(thumbrot_us/1000000.0f);
}