File content as of revision 6:405ec2bba6d0:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"

DigitalIn ButtonCal(D2);          //Button 1
DigitalOut gpo(D0);
DigitalOut ledred(LED_RED);
DigitalOut ledblue(LED_BLUE);
DigitalOut ledgreen(LED_GREEN);
AnalogIn pot1(A4);                  //POORTEN VERANDEREN
//AnalogIn pot2(A3);         //Beneden is I control op 0 gezet.   //POORTEN veranderen
AnalogIn pot3(A5);                  //POORTEN VERANDEREN
QEI encoder1(D10, D11, NC, 32);
QEI encoder2(D12, D13, NC, 32);
FastPWM Motor1PWM(D6);
DigitalOut Motor1Direction(D7);
FastPWM Motor2PWM(D5);            
DigitalOut Motor2Direction(D4);   
//EMG

AnalogIn    a0(A0);
AnalogIn    a1(A1);
AnalogIn    a2(A2);
AnalogIn    a3(A3);


/*AnalogIn Ppot(A0);
AnalogIn Ipot(A1);
AnalogIn Dpot(A2);
*/

MODSERIAL pc(USBTX, USBRX);

//HIDSCOPE          //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
HIDScope    scope(6);
Ticker      scopeTimer;

Ticker      EMGTicker;

// BiQuad filters
    //BiQuad Chains
BiQuadChain bqc1;
BiQuadChain bqc2;
BiQuadChain bqc3;
BiQuadChain bqc4;

    //High pass filters 20 Hz
BiQuad HP_emg1(1,-2,1,-1.647459981076977,0.700896781188403);
BiQuad HP_emg2(1,-2,1,-1.647459981076977,0.700896781188403); 
BiQuad HP_emg3(1,-2,1,-1.647459981076977,0.700896781188403); 
BiQuad HP_emg4(1,-2,1,-1.647459981076977,0.700896781188403);

    //Notch filters 50 Hz
BiQuad Notch_emg1(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
BiQuad Notch_emg2(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
BiQuad Notch_emg3(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003); 
BiQuad Notch_emg4(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);

    //LP 98
BiQuad LP1(1,2,1,1.911197067426073,0.914975834801434);
BiQuad LP2(1,2,1,1.911197067426073,0.914975834801434);
BiQuad LP3(1,2,1,1.911197067426073,0.914975834801434);
BiQuad LP4(1,2,1,1.911197067426073,0.914975834801434);



///Variables

double q1Error;
double q2Error;
double PrevErrorq1[100];
double PrevErrorq2[100];
int n = 100;            // Zelfde waarde als PrevErrorarray
double q1Ref = 1.0;     //VOOR TESTEN
double q2Ref;
double xRef = 40.0;
double yRef = 40.0;
double q1Pos;
double q2Pos;
double PIDerrorq1;
double PIDerrorq2;
double IntegralErrorq1 = 0.0;
double IntegralErrorq2 = 0.0;
double DerivativeErrorq1 = 0.0;
double DerivativeErrorq2 = 0.0;
double GainP1 = 3.0;
double GainI1 = 0.0;
double GainD1 = 0.235;
double GainP2 = 3.0;
double GainI2 = 0.0;
double GainD2 = 0.235;
double TickerPeriod = 1.0/500.0;
// Global variables EMG
double EMGdata1;
double EMGdata2;
double EMGdata3;
double EMGdata4;
int   count = 0;
double EMG_filt1;
double EMG_filt2;
double EMG_filt3;
double EMG_filt4;

double EMG_max1 = 10000.0; 
double EMG_max2 = 10000.0;
double EMG_max3 = 10000.0;
double EMG_max4 = 10000.0;

double unit_vX;   
double unit_vY;

Ticker Kikker;
int counter = 0;
int countstep = 0;

//Demo variabelen
double vxMax = 1.0;
double vyMax = 1.0;
int DemoStage = 0;

//States
enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState};
states State = WaitModusState;


//Calibration Time

int countcalibration = 0;
double CalibrationTime = 20.0; //Tijd om te calibreren seconden

//EMGDINGEN

void ReadEMG()
{

    EMGdata1 = a0.read(); // store values in the scope
    EMGdata2 = a1.read();
    EMGdata3 = a2.read();
    EMGdata4 = a3.read();

}

// EMG High pass filters
double EMG_HP1(double EMGdata) //data 1
{
    double HP_abs_EMGdata = bqc1.step(EMGdata);

    return fabs(HP_abs_EMGdata);
}

double EMG_HP2(double EMGdata) //data 2
{
    double HP_abs_EMGdata = bqc2.step(EMGdata);

    return fabs(HP_abs_EMGdata);
}

double EMG_HP3(double EMGdata) //data 3
{
    double HP_abs_EMGdata = bqc3.step(EMGdata);

    return fabs(HP_abs_EMGdata);
}

double EMG_HP4(double EMGdata) // data 4
{
    double HP_abs_EMGdata = bqc4.step(EMGdata);

    return fabs(HP_abs_EMGdata);
}

// EMG moving filter
double EMG_mean (double EMGarray[100])
{
    double sum = 0.0;

    for(int j=0; j<100; j++) 
        {
            sum += EMGarray[j];
        }

    double EMG_filt = sum / 100.0;
    
    return EMG_filt;
}

// Filtered signal to output between -1 and 1
double filt2kin (double EMG_filt1, double EMG_filt2, double max1, double max2)
{
    double EMG_scaled1 = EMG_filt1 / max1;
    double EMG_scaled2 = EMG_filt2 / max2;

    double kin_input = EMG_scaled2 - EMG_scaled1;

    if (kin_input > 1.0) {
        kin_input = 1.0;
    }
    if (kin_input < -1.0) {
        kin_input = -1.0;
    }

    return kin_input;
}

void EMG ()
{     
    ReadEMG();

    double HP_abs_EMGdata1 =  EMG_HP1(EMGdata1);
    double HP_abs_EMGdata2 =  EMG_HP2(EMGdata2);
    double HP_abs_EMGdata3 =  EMG_HP3(EMGdata3);
    double HP_abs_EMGdata4 =  EMG_HP4(EMGdata4);
    
    static double EMG_array1[100];
    static double EMG_array2[100];
    static double EMG_array3[100];
    static double EMG_array4[100];
    
    // Fill array 1
     for(int i = 100-1; i >= 1; i--) 
        {
            EMG_array1[i] = EMG_array1[i-1];
        }
    EMG_array1[0] = HP_abs_EMGdata1;
    
    // Fill array 2
     for(int i = 100-1; i >= 1; i--) 
        {
            EMG_array2[i] = EMG_array2[i-1];
        }
    EMG_array2[0] = HP_abs_EMGdata2;
    
    // Fill array 3
     for(int i = 100-1; i >= 1; i--) 
        {
            EMG_array3[i] = EMG_array3[i-1];
        }
    EMG_array3[0] = HP_abs_EMGdata3;
    
    // Fill array 4
     for(int i = 100-1; i >= 1; i--) 
        {
            EMG_array4[i] = EMG_array4[i-1];
        }
    EMG_array4[0] = HP_abs_EMGdata4;

    
    EMG_filt1 = EMG_mean(EMG_array1);         //global maken
    EMG_filt2 = EMG_mean(EMG_array2);
    EMG_filt3 = EMG_mean(EMG_array3);
    EMG_filt4 = EMG_mean(EMG_array4);

    

    unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2);
    unit_vY   = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4);
    if (fabs(unit_vX)<0.1)
    {
        unit_vX = 0.0;
    }
    if (fabs(unit_vY)<0.1)
    {
        unit_vY = 0.0;
    }
        

    scope.set(0, EMG_filt1);
    scope.set(1, EMG_filt2);
    scope.set(2, unit_vX);
    scope.set(3, EMG_filt3);
    scope.set(4, EMG_filt4);
    scope.set(5, unit_vY);
    

    count++;

    if (count == 100) 
    {
        pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref);
        count = 0;
    }
}



//PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS


//InverseKinematics

void inverse()
{
    double L1 = 40.0; // %define length of arm 1 attached to gear
    double L3 = sqrt(pow(xRef,2.0)+pow(yRef,2.0));
    double q3 = atan(yRef/xRef);
    double q4 = 2.0*asin(0.5*L3/L1);
    q1Ref = (0.5*3.1416-0.5*q4+q3)*9.0;
    q2Ref = (3.1416-q1Ref/9.0-q4)*4.0;
    }


void InverseKinematics ()// Kinematics function, takes imput between 1 and -1
{
    
    double V_max = 2.5; //Maximum velocity in either direction
    //     CM/s
    
    double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta
    double deltaY = TickerPeriod*V_max*unit_vY;
    
    xRef += deltaX;
    yRef += deltaY;
    
    inverse();
    
}


void ReadCurrentPosition()  //Update the coordinates of the end-effector q1Pos, q2Pos
{    
    q1Pos = -encoder1.getPulses()/32/131.25*2*3.1416     + 3.1416/2.0*9.0;        //Position motor 1 in rad
    q2Pos = encoder2.getPulses()/32/131.25*2*3.1416;        //Position motor 2 in rad 
}   

void UpdateError()    //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos
{
    q1Error = q1Ref - q1Pos;
    q2Error = q2Ref - q2Pos;

    //Update PrevErrorq1 and PrevErrorq2
    
    for (int i = 0; i <= n-2 ; i++)
    {
        PrevErrorq1[i] = PrevErrorq1[i+1];
        PrevErrorq2[i] = PrevErrorq2[i+1];
    }
    
    PrevErrorq1[n-1] = q1Error;
    PrevErrorq2[n-1] = q2Error;    
}

void PIDControl(){
    // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error
    
    
    // PID control motor 1
        //P-Control
        double P_error1 = GainP1 * q1Error;
     
        //I-Control
        IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod;
        double I_error1 = GainI1 * IntegralErrorq1;
     
        //D-Control
        //First smoothen the error signal
        double MovAvq1_1 = 0.0;
        double MovAvq1_2 = 0.0;
        for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
            MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1));
            MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1));
        }
        DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod;    
        double D_error1 = GainD1 * DerivativeErrorq1;
        // Derivative error sum over all time?   
     
    PIDerrorq1 = P_error1 + I_error1 + D_error1;
    
    // PID control motor 2
        //P-Control
        double P_error2 = GainP2 * q2Error;
     
        //I-Control
        IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod;
        double I_error2 = GainI2 * IntegralErrorq2;
     
        //D-Control
        //First smoothen the error signal
        double MovAvq2_1 = 0.0;
        double MovAvq2_2 = 0.0;
        for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
            MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1));
            MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1));
        }
        DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod;    
        double D_error2 = GainD2 * DerivativeErrorq2;
        // Derivative error sum over all time?   
     
    PIDerrorq2 = P_error2 + I_error2 + D_error2;
}

    
void MotorControl()
{
        //Motor 1 
        //Keep signal within bounds
        if (PIDerrorq1 > 1.0){  //tijdelijk 0.6, hoort 1.0 te zijn
            PIDerrorq1 = 1.0;
            }
        else if (PIDerrorq1 < -1.0){
            PIDerrorq1 = -1.0;
            }     
        //Direction    
        if (PIDerrorq1 <= 0){
            Motor1Direction = 1;
            Motor1PWM.write(-PIDerrorq1); //write Duty cycle 
        }        
        if (PIDerrorq1 >= 0){
            Motor1Direction = 0;
            Motor1PWM.write(PIDerrorq1); //write Duty cycle 
        }    
        
        //Motor 2 
        //Keep signal within bounds
        if (PIDerrorq2 > 1.0){
            PIDerrorq2 = 1.0;
            }
        else if (PIDerrorq2 < -1.0){
            PIDerrorq2 = -1.0;
            }     
        //Direction           
          
        if (PIDerrorq2 <= 0){
            Motor2Direction = 1;
            Motor2PWM.write(-PIDerrorq2); //write Duty cycle 
        }        
        if (PIDerrorq2 >= 0){
            Motor2Direction = 0;
            Motor2PWM.write(PIDerrorq2); //write Duty cycle 
        }    
}



void DemonstrationPath()
{
    
    // Also think about how to move from whatever position to (40,5)
    if (DemoStage == 0)    //From (40,40) to (40,-5)
    {
        if (yRef >0)
        {
            yRef = yRef - vyMax * TickerPeriod;
        }
        else
        {
            DemoStage = 1;
        }
    }
    else if (DemoStage == 1) //From (40,-5) to (65,-5)
    {
        if (xRef > 30)
        {
            xRef = xRef - vxMax * TickerPeriod;
        }
        else
        {
            DemoStage = 2;
        }
    }
    else if (DemoStage == 2)
    {
        if (yRef < 10)      //From (65,-5) to (65, 10)
        {
            yRef = yRef + TickerPeriod;
        }
        else
        {
            DemoStage = 3;
        }
    }
    else if (DemoStage == 3) //From (65,10) to (40,10)
    {
        if (xRef < 40)
        {
            xRef = xRef + vxMax * TickerPeriod;
        }
        else
        {
            DemoStage = 0;  // Repeat moving in the square pattern
        }
    }
}
void TestPath()
{
   /* GainP1 = Ppot.read()*60.0;
    GainI1 = Ipot.read()*20.0;
    GainD1 = Dpot.read();
    GainP2 = Ppot.read()*30.0;
    GainI2 = Ipot.read()*20.0;
    GainD2 = Dpot.read();
    
    pc.printf("GainP = %1f, GainI = %1f, GainD = %1f", GainP1, GainI1, GainD1);
    // Also think about how to move from whatever position to (40,5)
    if (DemoStage == 0)    //From (40,40) to (40,-5)
    {
        if (yRef >0)
        {
            yRef = yRef - vyMax * TickerPeriod;
        }
        else
        {
            DemoStage = 1;
        }
    }
    else if (DemoStage == 1)
    {
        if (yRef < 30)      //From (65,-5) to (65, 10)
        {
            yRef = yRef + vyMax * TickerPeriod;
        }
        else
        {
            DemoStage = 0;
        }
    }
    */
}



void CalibrationButton()
{
    ledred = 1;
    ledgreen = 1;
    ledblue = 0;
    
    EMG_max1 = 0.0001;
    EMG_max2 = 0.0001;
    EMG_max3 = 0.0001;
    EMG_max4 = 0.0001;
   
    State = EMGCalibrationState;
    
        
}

void EMGCalibration()
{
    if     (0.95*EMG_filt1>EMG_max1)
    {
        EMG_max1 = 0.95*EMG_filt1;
    }
    if     (0.95*EMG_filt2>EMG_max2)
    
    {
        EMG_max2 = 0.95*EMG_filt2;
    }
    
    if     (0.95*EMG_filt3>EMG_max3)
    {
            EMG_max3 = 0.95*EMG_filt3;
    }
    if     (0.95*EMG_filt4>EMG_max4)
    {
        EMG_max4 = 0.95*EMG_filt4;
    }
      
}



void CalibrationModus()
{
    
    EMG();    
    EMGCalibration();
    
    countcalibration++;
    pc.printf("countcal = %i", countcalibration);
    if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod))
    {
        State = NormalOperatingModusState;
        countcalibration = 0;
    }
    
}

void DemoModus()   // The ticker should call this function (in the switch statement)
{    

    //GainP1 = pot3.read() * 30;
    //GainI1 = pot2.read() * 10;
    //GainD1 = pot1.read() ;
    //GainP2 = pot3.read() * 10;
    //GainI2 = pot2.read() * 10;
    //GainD2 = pot1.read() ;
     
    //DemonstrationPath();
    TestPath();
    inverse();
    ReadCurrentPosition();
    UpdateError();
    PIDControl();
    MotorControl();
    
    //scope.set(0, q1Pos);
   // scope.set(1, q1Ref);
    //scope.set(2, q2Pos);
    //scope.set(3, q2Ref);
    
    count++;
    if (count ==400)
    {
    pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf,   xRef = %lf, yRef = %lf \n\r", GainP1, GainI1, GainD1, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); 
    count = 0;
    }
}


void NormalOperatingModus()
{
     ledred = 1;
     ledgreen = 0;
     ledblue = 1;
     
     EMG();
     InverseKinematics();
     ReadCurrentPosition();
     UpdateError();
     PIDControl();
     MotorControl();
     
     //scope.set(0, q1Pos);
     //scope.set(1, q1Ref);
     
     //GainP1 = pot3.read() * 10;
     //GainI1 = pot2.read() * 10;
     //GainD1 = pot1.read() ;
          
     countstep++;
     counter++;
     if (counter == 400) // print 1x per seconde waardes.
    {
        //pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref);
        counter = 0;   
    }
     if (countstep == 4000)
     {
         q1Ref = !q1Ref;
         countstep = 0;
     }    
         
    
}


 
void StateMachine()
{
    
    
    if (ButtonCal.read() == 0)
    {
        CalibrationButton();
        pc.printf("print iets");
    }
        
    switch(State)
    {
        case WaitModusState: //aanmaken
                EMG();
                pc.printf("Wait \n\r");
                break;
        case EMGCalibrationState:
             CalibrationModus(); 
             //pc.printf("EMG CAL \n\r");          
                break;
        case NormalOperatingModusState:
                NormalOperatingModus();
                //pc.printf("NOMS \n\r");
                break;
        case DemoModusState:
             DemoModus();    
             pc.printf("Demo \n\r");  
                break;
        default :        
    }
}    
                                        
         
 
int main()
{
        pc.baud(115200);
        
        /*
        GainP1 = pot3.read() * 10;
        GainI1 = pot2.read() * 10;
        GainD1 = pot1.read();
        */
        
        pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1);
        wait(7.0);
        pc.printf("nog 3 seconden \n\r");
        wait(3.0);
        
        
        //BiQuad chains
        bqc1.add( &HP_emg1 ).add( &Notch_emg1 );//.add( &LP1);
        bqc2.add( &HP_emg2 ).add( &Notch_emg2 );//.add( &LP2);
        bqc3.add( &HP_emg3 ).add( &Notch_emg3 );//.add( &LP3);
        bqc4.add( &HP_emg4 ).add( &Notch_emg4 );//.add( &LP4);
        
        //Initialize array errors to 0
        for (int i = 0; i <= 9; i++){
             PrevErrorq2[i] = 0;
             PrevErrorq2[i] = 0;
             }
             
        int frequency_pwm = 16700; //16.7 kHz PWM
        Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
        Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
        
       //Emg Calibratie button
        //ButtonCal.fall(&CalibrationButton);
        
        
        
        Kikker.attach(StateMachine, TickerPeriod);
       // scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
        
        //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!!
        // Attach the HIDScope::send method from the scope object to the timer at 50Hz
        scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
        //EMGTicker.attach_us(EMG_filtering, 5e3);
        //.
        
        while(true);
        {}        
}