File content as of revision 18:7178416f2db5:

#include "mbed.h"
#include "QEI.h"

#include "servoController.h"
#include "emg.h"

#define pi 3.14159265359;

PwmOut Motor1_pwm(D5);
DigitalOut SlideMotor_Direction(D4);
PwmOut Motor2_pwm(D6);
DigitalOut LiftMotor_Direction(D7);
AnalogIn Potmeter(A0);
AnalogIn Potmeter2(A1);
QEI Slide_Encoder(D12,D13,NC,64);
QEI Lift_Encoder(D10,D11,NC,64);

BiQuadChain Hz_1;
BiQuad bq0(0.05852855368, 0.11705710736, 0.05852855368,-1.05207469728, 0.28586907478);
BiQuad bq1(0.06463239794, 0.12926479589, 0.06463239794,-1.16338171052, 0.42191097989);
BiQuad bq2(0.07902502847, 0.15805005694, 0.07902502847,-1.42439823874, 0.7409311811);

Serial pc(USBTX,USBRX);
Ticker Controller;

bool Controller_Flag=0;
float Frequency = 30;
float Frequency_PWM = 10000;

float Slide_Radius = 12.5;
float Slide_Multiplier = 1;
float k1 = 1;
float k2 = 0.1f;
float k3 = 0.1f;
float Start_slow = 40;
float Start_lock = 0;
float End_slow = 340;
float End_lock = 380;
float Slide_Counts;
float Slide_Revolutions;
float Slide_Angle;
float Slide_Position;

float Slide_Input_force = 0;
float Slide_Curr_speed = 0;
float Slide_Desired_speed;
float Slide_Delta_speed;
float Slide_Int_delta_speed;
float Slide_Deriv_delta_speed = 0;
float Slide_Prev_delta_speed = 0;
float Slide_PI;

float Lift_Radius = 10;
float Lift_Multiplier = 1;
float Lift_k1 = 0.2;
float Lift_k2 = 0.05;
float Lift_k3 = 0.01;
float Lift_Start = 0;
float Lift_End = 50;
float Lift_Counts;
float Lift_Revolutions;
float Lift_Angle;
float Lift_Position;

float Lift_Input_force = 0;
float Lift_Desired_position;
float Lift_Delta_position;
float Lift_Int_delta_position;
float Lift_Deriv_delta_position = 0;
float Lift_Prev_delta_position = 0;
float Lift_PI;
bool Lift_Switch = false;
Timeout Lift_Timeout;
bool Lift_Timeout_Switch = true;

// state machine variables
float statechange_threshold = 0.6;
bool binary_norm_emg_2 = 0;        
bool binary_norm_emg_2_previous = 0;
states scoopstate = STATE_MOVE;  // all states are defined in one place in emg.h
bool state_resettime_value = 1;
Timeout state_resettime;
DigitalOut LED_STATE_MOVE(D15);
DigitalOut LED_STATE_GRAB(D14);
Timeout controlstarterTimeout;

//callable functions
void Slide_Controller();
void Lift_Controller();
void Ticker_Flag();
void set2true();
void change_scoopstate();
void start_controlling();

int main()
{
    Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
    Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f

    pc.baud(9600);
    
    led.write(1);
    Lift_Input_force =  Potmeter.read();
    Slide_Input_force = Potmeter2.read();
    
    // define the filters used in main
    notch_50_0.add( &bq03 ).add( &bq04 ).add( &bq05 );
    notch_50_1.add( &bq13 ).add( &bq14 ).add( &bq15 );
    notch_50_2.add( &bq23 ).add( &bq24 ).add( &bq25 );
    high_pass_0.add( &bq06 ).add( &bq07 );
    high_pass_1.add( &bq16 ).add( &bq17 );
    high_pass_2.add( &bq26 ).add( &bq27 );
    low_pass_0.add( &bq9 ).add( &bq10 ).add( &bq11 );
    low_pass_1.add( &bq19 ).add( &bq110 ).add( &bq111 );
    low_pass_2.add( &bq29 ).add( &bq210 ).add( &bq211 );
    
    //call the timeouts for the startup states and the ticker for reading the emgs
    change_state.attach( &calibrate,1);
    change_state2.attach( &run,11);
    controlstarterTimeout.attach(&start_controlling,11);
    emgSampleTicker.attach( &emgSample, 0.005);         //200Hz
    ServoPWMpin.period(0.01f);                          // 0.01 second period of pwm cycle
    
    while (true) {
        if (go_emgSample == true){
                EMG_filter();
        }
        
        if (Controller_Flag == true){
            if(Norm_EMG_2 > statechange_threshold){
                binary_norm_emg_2 = 1;
            }else{
                binary_norm_emg_2 = 0;
            }
            if((binary_norm_emg_2 != binary_norm_emg_2_previous)&&binary_norm_emg_2&&state_resettime_value){
                change_scoopstate();
            }
            binary_norm_emg_2_previous = binary_norm_emg_2;
            
            switch(scoopstate){
                case STATE_MOVE:
                    Slide_Controller();
                    LED_STATE_MOVE = 1;
                    LED_STATE_GRAB = 0;
                    break;
                case STATE_GRAB:
                    Lift_Controller();
                    control_servo(Norm_EMG_1);
                    LED_STATE_GRAB = 1;
                    LED_STATE_MOVE = 0;
                    break;
            }          
            Controller_Flag = false;
        }
    }
    return 0;
}

void set2true(){
    state_resettime_value = 1;
}

void change_scoopstate(){
    Motor1_pwm.write(0);
    Motor2_pwm.write(0);
    switch(scoopstate){
        case STATE_MOVE:
            scoopstate = STATE_GRAB;
            break;
        case STATE_GRAB:
            scoopstate = STATE_MOVE;
            break;
    }
    state_resettime.attach(&set2true, 1);
    state_resettime_value = 0;
}

void start_controlling(){
    Controller.attach(&Ticker_Flag,1/Frequency);
}   

void Ticker_Flag(){
    Controller_Flag = true;
}

void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)

    Slide_Counts =      Slide_Encoder.getPulses();
    Slide_Revolutions = Slide_Counts /(32*131);
    Slide_Angle =       Slide_Revolutions*2*pi;
    Slide_Position =    Slide_Angle*Slide_Radius + 135;
            
    Slide_Desired_speed= (-Norm_EMG_1+Norm_EMG_0)*Slide_Multiplier;
    
    
    if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
        Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
        
    }
    if (Slide_Position > End_slow && Slide_Desired_speed < 0){
       Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
    }
    
    Slide_Prev_delta_speed = Slide_Delta_speed;
    Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed;                   // P
    Slide_Int_delta_speed += Slide_Delta_speed/Frequency;                       // I
    if (Slide_Int_delta_speed > 1){Slide_Int_delta_speed = 1;}
    if (Slide_Int_delta_speed < -1){Slide_Int_delta_speed = -1;}
    Slide_Int_delta_speed *= 1/1.3;
    pc.printf("%f - %f - %f \r\n",Norm_EMG_0, Norm_EMG_1, Slide_Delta_speed);
    Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
    if (Slide_PI<0){
        SlideMotor_Direction = 0;
    }else{
        SlideMotor_Direction = 1;
    }   
    
    Motor1_pwm.write(abs(Slide_PI));
    //return k1*Delta_speed + k2*Int_delta_speed;
}

void Lift_Timeout_Return(){
        Lift_Timeout_Switch = true;
    }

void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
    
    Lift_Counts =       Lift_Encoder.getPulses();
    Lift_Revolutions =  Lift_Counts /(32*131);
    Lift_Angle =        Lift_Revolutions*2*pi;
    Lift_Position =     Lift_Angle*Lift_Radius;
    
    if(Norm_EMG_0 > 0.6 && Lift_Timeout_Switch == true){
        Lift_Switch = !Lift_Switch;
        Lift_Timeout_Switch = false;
        Lift_Timeout.attach(Lift_Timeout_Return, 1);
    }
    
    Lift_Desired_position = Lift_Switch*150;
    //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
    Lift_Prev_delta_position = Lift_Delta_position;                             
    Lift_Delta_position = Lift_Desired_position-Lift_Position;             // P
    Lift_Int_delta_position += Lift_Delta_position/Frequency;                   // I
    
    Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;   //D
    if (Lift_Int_delta_position > 1){Lift_Int_delta_position = 1;}
    if (Lift_Int_delta_position < -1){Lift_Int_delta_position = -1;}
    
    Lift_PI = Lift_k1*Lift_Delta_position + 0*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
    if (Lift_PI<0){
        LiftMotor_Direction = 1;
    }else{
        LiftMotor_Direction = 0;
    }   
    
    Motor2_pwm.write(abs(Lift_PI));
    //return k1*Delta_speed + k2*Int_delta_speed;
}