File content as of revision 3:0c31a4a5d1fe:

// Robot states
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "biquadFilter.h" 
#include "math.h" 
#include "FastPWM.h"

// Define objects
AnalogIn emg1(A0);
AnalogIn emg2(A1);
AnalogIn emg3(A2);
AnalogIn emg4(A3);
MODSERIAL pc(USBTX,USBRX);
DigitalOut led_red(LED_RED);
DigitalOut led_blue(LED_BLUE);
DigitalOut led_green(LED_GREEN);
InterruptIn button_Mbed(PTC6); //Button 1 Mbed
InterruptIn button_1(); //Button 2 BRS
InterruptIn button_2(); // Button 3 BRS

FastPWM motor1(D6);
DigitalOut motor1_dir(D7);
FastPWM motor2(D5);
DigitalOut motor2_dir(D4);


//ticker setup
Ticker states_machine;

    
//encoder setup
QEI encoder_1(D13,D12,NC,32,QEI::X4_ENCODING);
QEI encoder_2(D11,D10,NC,32,QEI::X4_ENCODING);

//motor setup
int motordir1 = 1;
int motordir2 = 1;
double Kp = 17.5;

//Hidscope setup
HIDScope    scope(4); 

// EMG setup
biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
double emg_value_1;
double signalpart1_1;
double signalpart2_1;
double signalpart3_1;
double signalpart4_1;
double signalfinal_1;
double emgsignal_1;
double onoffsignal_1;
double maxcal_1=0;        

double emg_value_2;
double signalpart1_2;
double signalpart2_2;
double signalpart3_2;
double signalpart4_2;
double signalfinal_2;
double emgsignal_2;
double onoffsignal_2;
double maxcal_2=0; 

double emg_value_3;
double signalpart1_3;
double signalpart2_3;
double signalpart3_3;
double signalpart4_3;
double signalfinal_3;
double emgsignal_3;
double onoffsignal_3;
double maxcal_3=0; 

double emg_value_4;
double signalpart1_4;
double signalpart2_4;
double signalpart3_4;
double signalpart4_4;
double signalfinal_4;
double emgsignal_4;
double onoffsignal_4;
double maxcal_4=0; 

double emgx;
double emgy;
double kemg = 0.1;

//kinematics setup
double xgoal;
double ygoal;
double theta1;
double theta2;

   
// State machine
enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
states CurrentState = START;
bool StateChanged = true;         // this is the initialization of the first state

// button functions

void emgread(){
     emg_value_1 = emg1.read();//read the emg value from the elektrodes
        signalpart1_1 = notch.step(emg_value_1);//Highpass filter for removing offset and artifacts
        signalpart2_1 = filterhigh1.step(signalpart1_1);//rectify the filtered signal
        signalpart3_1 = abs(signalpart2_1);//low pass filter to envelope the emg
        signalpart4_1 = filterlow1.step(signalpart3_1);//notch filter to remove 50Hz signal
        emgsignal_1 = filterlow2.step(signalpart4_1);//2nd low pass filter to envelope the emg
     emg_value_2 = emg2.read();//read the emg value from the elektrodes
        signalpart1_2 = notch.step(emg_value_2);//Highpass filter for removing offset and artifacts
        signalpart2_2 = filterhigh1.step(signalpart1_2);//rectify the filtered signal
        signalpart3_2 = abs(signalpart2_2);//low pass filter to envelope the emg
        signalpart4_2 = filterlow1.step(signalpart3_2);//notch filter to remove 50Hz signal
        emgsignal_2 = filterlow2.step(signalpart4_2);//2nd low pass filter to envelope the emg
    emg_value_3 = emg3.read();//read the emg value from the elektrodes
        signalpart1_3 = notch.step(emg_value_3);//Highpass filter for removing offset and artifacts
        signalpart2_3 = filterhigh1.step(signalpart1_3);//rectify the filtered signal
        signalpart3_3 = abs(signalpart2_3);//low pass filter to envelope the emg
        signalpart4_3 = filterlow1.step(signalpart3_3);//notch filter to remove 50Hz signal
        emgsignal_3 = filterlow2.step(signalpart4_3);//2nd low pass filter to envelope the emg
    emg_value_4 = emg4.read();//read the emg value from the elektrodes
        signalpart1_4 = notch.step(emg_value_4);//Highpass filter for removing offset and artifacts
        signalpart2_4 = filterhigh1.step(signalpart1_4);//rectify the filtered signal
        signalpart3_4 = abs(signalpart2_4);//low pass filter to envelope the emg
        signalpart4_4 = filterlow1.step(signalpart3_4);//notch filter to remove 50Hz signal
        emgsignal_4 = filterlow2.step(signalpart4_4);//2nd low pass filter to envelope the emg
}
void emgcal(){
            emgread();
            double signalmeasure_1 = emgsignal_1;
            if (signalmeasure_1 > maxcal_1){//determine what the highest reachable emg signal is
                maxcal_1 = signalmeasure_1;}
            double signalmeasure_2 = emgsignal_2;
            if (signalmeasure_2 > maxcal_2){//determine what the highest reachable emg signal is
                maxcal_2 = signalmeasure_2;}
            double signalmeasure_3 = emgsignal_3;
            if (signalmeasure_3 > maxcal_3){//determine what the highest reachable emg signal is
                maxcal_3 = signalmeasure_3;}
            double signalmeasure_4 = emgsignal_4;
            if (signalmeasure_4 > maxcal_4){//determine what the highest reachable emg signal is
                maxcal_4 = signalmeasure_4;}
            scope.set(0,signalmeasure_1);//set emg signal to scope in channel 1
            scope.set(1,signalmeasure_2);//set filtered signal to scope in channel 2
            scope.set(2,signalmeasure_3);//set filtered signal to scope in channel 3
            scope.set(3,signalmeasure_4);//set filtered signal to scope in channel 4
        }
    

void emgshow(){
        emgread();
        onoffsignal_1=emgsignal_1/maxcal_1;// emg positive x
        onoffsignal_2=emgsignal_2/maxcal_2;// emg negative x   
        onoffsignal_3=emgsignal_3/maxcal_3;// emg positive y
        onoffsignal_4=emgsignal_4/maxcal_4;// emg negative y
        scope.set(0,onoffsignal_1);//set emg signal to scope in channel 1
        scope.set(1,onoffsignal_2);//set filtered signal to scope in channel 2
        scope.set(2,onoffsignal_3);//set filtered signal to scope in channel 3
        scope.set(3,onoffsignal_4);//set filtered signal to scope in channel 4
        emgx = onoffsignal_1- onoffsignal_2;
        emgy = onoffsignal_3- onoffsignal_4;
    scope.send();
        }

void forwardkin(){
    double qref2 = encoder_2.getPulses();
    double q2_correct = (qref2*2*3.14)/8400.0;
    double qref1 = encoder_1.getPulses();
    double q1_correct = (qref1*2*3.14)/8400.0;
    double x = 0.3*cos(q1_correct)+0.3*cos(q1_correct+q2_correct);
    double y = 0.3*sin(q1_correct)+0.3*cos(q1_correct+q2_correct);
    emgshow();
    xgoal = x + emgx* kemg;
    ygoal = y + emgy* kemg;
    }
    
void reversekin(){
    forwardkin();
    theta2= acos(((xgoal*xgoal)+(ygoal*ygoal)-(0.3*0.3)-(0.3*0.3))/(2*0.3*0.3));
    theta1= atan(ygoal/xgoal)-atan((0.3*sin(theta2))/(0.3+0.3*cos(theta2)));
    }
    
void motor_position()
{
    reversekin();
    double pos_1 = encoder_1.getPulses();
    double poscorrect_1 = (pos_1*3.14*2)/8400.0;
    double error1 = theta1-poscorrect_1;
    if (error1 >=0) motor1_dir=1;
    else motor1_dir=0;
    if (fabs(error1)>1) motor1 = 1;
    else motor1 = fabs(error1);
    
    double pos_2 = encoder_2.getPulses();
    double poscorrect_2 = (pos_2*3.14*2)/8400.0;
    double error2 = theta2-poscorrect_2;
    if (error2 >=0) motor2_dir=1;
    else motor1_dir=0;
    if (fabs(error2)>1) motor2 = 1;
    else motor1 = fabs(error2);
    }

void kalmot(){
    CurrentState = KAL_ME;
    StateChanged = true;
}
           
void kalemg(){
    CurrentState = KAL_EMG;
    StateChanged = true;
    wait(0.2f);
}
void movestart(){
    CurrentState = MOVE_START;
    StateChanged = true;
    wait(0.2f);
    }
    
void readystart(){
    CurrentState = READY_START;
    StateChanged = true;
    wait(0.2f);
    }
void demo(){
    CurrentState = DEMO;
    StateChanged = true;
    wait(0.2f);
    }
void move(){
    CurrentState = MOVE;
    StateChanged = true;
    wait(0.2f);
    }
void wait(){
    CurrentState = WAIT;
    StateChanged = true;
    wait(0.2f);
    }
void off(){
    CurrentState = OFF;
    StateChanged = true;
    wait(0.2f);
    }
    
//led functions
void flashred(){
    led_red = !led_red;
    wait(0.4f);
    }
void flashgreen(){
    led_green = !led_green;
    wait(0.4f);
    }    
void flashblue(){
    led_blue = !led_blue;
    wait(0.4f);
    } 
// Function START_TO_KAL_ME

void StateMachine(void)
{
    switch(CurrentState)
    {
        case START:     
            if (StateChanged)
            {
                pc.printf("Start state, red led is on. If button 1 pressed, go to kal_me state");
                led_red = 0;  // Red led is on 
                led_blue = 1;
                led_green = 1;
                StateChanged = false;
            }
            button_Mbed.rise(&kalmot);  // State switches when button pressed
                           
        break;      // end of state START
                
        case KAL_ME:
            if (StateChanged)
            {
                pc.printf("Calibration ME state, red ld flickers slow"); 
                flashred();
                                
                
                // FUNCTION Move to mechanical stop, include v_motor, t_passed
                // FUNCTION Reset encoders
                
                StateChanged = false;
            }
            
            button_Mbed.rise(&kalemg);
        break;      // end of state KAL_ME
        
        case KAL_EMG:
            if (StateChanged)
            {
                flashgreen();
                emgcal();              
                StateChanged = false;
            }
            
            button_Mbed.rise(&readystart);
            
        break;      // end of state KAL_EMG
        
        case MOVE_START:
            if(StateChanged)
            {
                led_red = 1;
                led_blue = 1; 
                led_green = 0;   // Green led is on
            
                // FUNCTION move to start, t_passed
                // Define current_position & start_position 
                StateChanged = false; 
            }
            
            button_Mbed.rise(&readystart);
            
        break;      // end of state MOVE_START
        
        case READY_START:
            if (StateChanged)
            {
                led_red = 1;
                led_blue = 1;
                led_green = 0;       // Green led is on 
                
                StateChanged = false;
            }
            
            button_Mbed.rise(&demo);
            //button_1.rise(&move);
                           
        break;      // end of state READY_START
        
        case DEMO:
            if (StateChanged)
            {
                //FUNCTION Blue led blink fast
                
                //FUNCTION perform straight movements for demo
                
                StateChanged = false; 
            }
            
            button_Mbed.rise(&movestart);
        break;      // end of state DEMO
        
        case MOVE:
            if (StateChanged)
            {
                led_red = 1;
                led_green = 1;
                led_blue = 0;  //Blue led is on 
                
                // FUNCTION Play the game with EMG signal
                
                StateChanged = false; 
            }
            
            button_Mbed.rise(&off);
            button1.rise(&wait);
            
            break;      // end of state MOVE
        
        case WAIT:
            if (StateChanged)
            {
                led_red = 0;   // Pink led iS on 
                led_blue = 0; 
                led_green = 1
                
            button_Mbed.rise(&off);
            button1.rise(&movestart);
            button2.rise(&move);
           
            
        break;      // end of state WAIT
        
        case OFF:
            led_red = 0;       // White led is on 
            led_blue = 0; 
            led_green = 0;
            
        break;      // end of state OFF
        
        default:
        TurnMotorsoff(); //FUNCTION 
        printf("Unknown state reached");
    }               // End of the switch, all states are prescribed
}

int main(void)      // wat hier in moet snap ik nog niet 
{
        states_machine.attach(&StateMachine, 0.002)
        // hier moeten dingen komen
        while (true)
        {
            CheckForCommandFromTerminal();
        }
}