File content as of revision 7:af586102d80c:

#include "mbed.h"
#include <stdio.h>
#include "QEI.h"
//#include "HIDScope.h"
//#include "MODSERIAL.h"
#include "BiQuad.h"
#include "math.h"
#include "Constants.h"

#define IGNORECOUNT 100
#define PI 3.141592f //65358979323846  // pi 

void yDirection() {
    //direction of the motion
    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
        direction = -1;
    }
    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
        direction = 1;
    }

    if (emg0Bool || emg1Bool){  
        //correction from motor 1 to keep x position the same
        
        
        //calculating the motion
        old_y_position = y_position;
        y_position = old_y_position + (0.0003f * direction);
        
            
        old_motor1_angle = motor1_angle;
        motor1_angle = asin( y_position / length )* C1;     //saw tooth motor angle in rad
        if (y_position > 0.29f){
            y_position = 0.29f;
        }
        else if(y_position <0){
            y_position = 0;
        }
        //correction on x- axis
        old_x_correction = x_correction;
        x_correction = old_x_correction - (length * cos(old_motor1_angle / C1)- length * cos(motor1_angle/ C1)); // old x + correction
        if (x_position > 0.35f- x_correction ){
            x_position = 0.35f- x_correction;
        }
        else if (x_position-x_correction < 0.0f){
            x_position = 0.0f;
        }
        
        motor2_angle = ( x_position + x_correction - (length * cos(motor1_angle /C1 ))) / C2;
        //reset the boolean, for demo purposes
        emg2Bool = false; 
    }
}

void xDirection () {      
    //if the button is pressed, reverse the y direction
    if (!button2) {
        x_direction = !x_direction;
        wait(0.5f);
        }     
        
    if (emg2Bool) { //if w is pressed, move up/down
        //direction of the motion
        if (x_direction) { 
            direction = 1.0f;
        }
        else if (!x_direction) {
            direction = -1.0f;
        }
        
        //calculating the motion
        old_x_position = x_position;
        x_position = old_x_position + (0.0003f * direction);
        if (x_position > 0.35f- x_correction ){
            x_position = 0.35f- x_correction;
        }
        else if (x_position-x_correction < 0.0f){
            x_position = 0.0f;
        }

        motor2_angle =( x_position + x_correction - (length * cos( motor1_angle / C1 ))) /C2 ; // sawtooth-gear motor angle in rad
        
        //reset the boolean, for demo purposes
        emg2Bool = false; 
    }
}

void PIDController1() {  
    P1 = motor1.getPulses() / 8400.0f * 2.0f * PI;    //actual motor angle in rad  
    e1 = e2;
    e2 = e3;
    e3 = motor1_angle - P1;
    de3 = (e3-e2)/Timestep;
    ie3 = ie3 + e3*Timestep;
    Output1 = Kp * e3 + Ki * ie3 + Kd * de3;
    
    Y1 = 0.5f * Output1;
    
    if (Y1 >= 1){
        Y1 = 1;
    }
    else if (Y1 <= -1){
        Y1 = -1;
    }       
}

void PIDController2() {   
    P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad
    f2 = f3;
    f3 = motor2_angle - P2;
    df3 = (f3-f2)/Timestep;
    if3 = if3 + f3*Timestep;
    Output2 = Kp * f3 + Ki * if3 + Kd * df3;
    Y2 = 0.5f * Output2;
     
    if (Y2 >= 1){
        Y2 = 1;
    }
    else if (Y2 <= -1){
        Y2 = -1;
    }   
}

void ControlMotor1() {
    if (Y1 > 0.0f) {
        Y1 = 0.6f * Y1 + 0.4f;
        directionpin1 = false;
    } 
    else if(Y1 < -0.0f){
        Y1 = -0.4f + 0.6f * Y1;
        directionpin1 = true;
    }    

    pwm_value1 = fabs(Y1);
}

void ControlMotor2() {
    if (Y2 > 0.0f) {
        Y2 = 0.6f * Y2 + 0.4f;
        directionpin2 = true;
    } 
    else if(Y2 < -0.0f){
        Y2 = -0.4f + 0.6f * Y2;
        directionpin2 = false;
    }

    pwm_value2 = fabs(Y2);
}

/*
* Calibration functions
*/
void positionCalibration() {
    while(!button1) {
        directionpin2 = true;
        pwmpin1 = 0.5f;
    }
    while(!button2) {
        directionpin1 = true;
        pwmpin2 = 0.4f;
    }
    pwmpin1 = 0.0f;
    pwmpin2 = 0.0f;
    
    if(!next_switch) {
        CurrentState = Movement;
        motor1.reset();
        motor2.reset();
    }
}

void CalibrateEMG0(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
    ledred = !ledred;
    int C = 500;           // half a second at 1000Hz
    double A0=0, A1=0, A2=0, A3=0, A4=0;
    double emg0FiAbs;
    while (C > 0){
        emg0FiAbs = fabs( emg1bqc.step(emg1.read()));
        if (C==500){                        //first instance make all values the first in case this is the highest
            A0=A1=A2=A3=A4=emg0FiAbs;
        }
        else if(emg0FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
            A4=A3;
            A3=A2;
            A2=A1;
            A1=A0;
            A0=emg0FiAbs;
        }
        C--;
        wait(0.001f);
    }
    Calibration0 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
    ledred = !ledred;
}  

void CalibrateEMG1(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
    ledred = !ledred;
    int C = 500;           // half a second at 1000Hz
    double A0=0, A1=0, A2=0, A3=0, A4=0;
    double emg1FiAbs;
    while (C > 0){
        emg1FiAbs = fabs( emg1bqc.step(emg1.read()));
        if (C==500){                        //first instance make all values the first in case this is the highest
            A0=A1=A2=A3=A4=emg1FiAbs;
        }
        else if(emg1FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
            A4=A3;
            A3=A2;
            A2=A1;
            A1=A0;
            A0=emg1FiAbs;
        }
        C--;
        wait(0.001f);
    }
    Calibration1 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
    ledred = !ledred;
}  

void CalibrateEMG2(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
    ledred = !ledred;
    int C = 500;           // half a second at 1000Hz
    double A0=0, A1=0, A2=0, A3=0, A4=0;
    double emg2FiAbs;
    while (C > 0){
        emg2FiAbs = fabs( emg2bqc.step(emg2.read()));
        if (C==500){                        //first instance make all values the first in case this is the highest
            A0=A1=A2=A3=A4=emg2FiAbs;
        }
        else if(emg2FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
            A4=A3;
            A3=A2;
            A2=A1;
            A1=A0;
            A0=emg2FiAbs;
        }
        C--;
        wait(0.001f);
    }
    Calibration2 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
    ledred = !ledred;
}    

/*
* filter functions
*/

bool emg0Filter(void){  
    double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute, 
    /* this is the threshhold */ 
    if (emg0filteredAbs > Calibration0) {  // when above threshold  set bool to 1, here can the parameters be changed using global variables
        emg0Bool = 1;
        emg0Ignore = IGNORECOUNT;   // here is the counter reset to catch sudden jolts
        }
    else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
         emg0Bool = 0;
        }
    else {
        emg0Ignore--;      // else decrease counter by one each time has passed without threshold being met
        }  
    return emg0Bool;
}

bool emg1Filter(void){  
    double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
    /* this is the threshhold */ 
    if (emg1filteredAbs > Calibration1) {  // when above threshold  set bool to 1 here can the parameters be changed using global variables
        emg1Bool = true;
        emg1Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
        }
    else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
         emg1Bool = false;
        }
    else {
        emg1Ignore--;      // else decrease counter by one each time has passed without threshold being met
        }
    return emg1Bool;
}

bool emg2Filter(void){  
    double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
    /* this is the threshhold */ 
    if (emg2filteredAbs > Calibration2) {  // when above threshold  set bool to 1 here can the parameters be changed using global variables
        emg2Bool = true;
        emg2Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
        }
    else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
        emg2Bool = false;
        }
    else {
        emg2Ignore--;      // else decrease counter by one each time has passed without threshold being met
        }

    return emg2Bool;
}

void sample() {  
    //run the filter functions for each emg signal
    emg0Filter();
    emg1Filter();
    emg2Filter();
}

/*
* main function for the emg calibration
*/
void emgCalibration() {
    // combining biquad chains is done in main, before the ticker
    emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 );       
    emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
    emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 );
    
    bool u = true;
    CalibrationState = EMG0;
    while (u){                      // !!! this is a place holder for the calibration!!! 
        switch(CalibrationState) {
            case EMG0:
            if(!next_switch) {   
                CalibrateEMG0(); // calibration function
                CalibrationState = EMG1;
            }
            break;
            case EMG1:
            if(!next_switch) {   
                CalibrateEMG1(); // calibration function
                CalibrationState = EMG2;
            }
            break;
            case EMG2:
            if(!next_switch) {      
                CalibrateEMG2(); // calibration function
                CurrentState = PositionCalibration;
                sample_timer.attach(&sample, 0.001);    //ticker at 1000Hz
                ledred = false;     // to indicate filter is working
                ledgreen = false;
                ledblue = false;
                u = false;
            }
            break;
        }
    }     
}

/*
* function that runs the Kinematics and PID controller
*/
void movement() {
    xDirection(); //call the function to move in the y direction
    yDirection(); //call the function to move in the x direction
    PIDController1();
    PIDController2();
    ControlMotor1();
    ControlMotor2();
}

void move_motors() {
    // assign the caltulates pwm values to the pwm pins
    pwmpin1 = pwm_value1;
    pwmpin2 = pwm_value2;
}

/*
* the "main" function that runs the state machine
*/
void StateFunction() {
    switch(CurrentState) {            
    case EmgCalibration:
    ledgreen = false; //turn on the green light
    emgCalibration();
    break;
    
    case PositionCalibration:
    ledred = true;
    ledgreen = true;
    ledblue = false; //turn on the blue light
    positionCalibration();
    break;
    
    case Movement:
    ledred = true;
    ledgreen = false; //turn on a cyan light
    ledblue = false;
    movement();
    break;
    }
}

void KillInterrupt() {
    //detach all the tickers
    MotorsTicker.detach();
    sample_timer.detach();
    stateticker.detach();
    
    //turn off the motors
    pwm_value1 = 0.0f;
    pwm_value2 = 0.0f;
    pwmpin1 = 0.0f;
    pwmpin2 = 0.0f;
    
    //burn the red light
    ledred = false;
    ledgreen = true;
    ledblue = true;
    
    bool b = true;
    
    //have the program get stuck in a while loop 
    //to be sure that when an interrupt changes the pwmvalues somehow, they get turned off immediately
    while(b){ 
        pwm_value1 = 0;
        pwm_value2 = 0;
        if (!next_switch){
            CurrentState = PositionCalibration;
            b = false;
        }
    }
    wait(1.5f);
    MotorsTicker.attach(&move_motors, Timestep);
    stateticker.attach(StateFunction, Timestep);
}

int main()
{
    pwmpin1.period_us(60);
    pwmpin2.period_us(60);
    directionpin1 = true;
    directionpin2 = true;
    
    ledred = true;
    ledgreen = true;
    ledblue = true;
        
    kill_switch.fall(KillInterrupt); //attach the kill switch to the KillInterrupt function
    MotorsTicker.attach(&move_motors, Timestep); //ticker at 50Hz
    stateticker.attach(StateFunction, Timestep);

    CurrentState = EmgCalibration; //start at the calibration state
    
    while (true) {      
    }
}