File content as of revision 5:b9d5d7311dac:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"

DigitalOut  motor1_direction(D4);
PwmOut      motor1_speed(D5);
PwmOut      led(D9);
DigitalIn   button_1(PTC6); //counterclockwise
DigitalIn   button_2(PTA4); //clockwise
AnalogIn    PotMeter1(A5);
AnalogIn    EMG(A0);
//AnalogIn    EMG_bicepsright (A1);
//AnalogIn    EMG_legleft (A2);
//AnalogIn    EMG_legright (A3);
Ticker      controller;
Ticker      ticker_regelaar;
Ticker      EMG_Control;
//Timer       Timer_Calibration;
Encoder     motor1(D12,D13);
HIDScope    scope(3);

MODSERIAL   pc(USBTX, USBRX);
volatile bool regelaar_ticker_flag;

void setregelaar_ticker_flag()
{
    regelaar_ticker_flag = true;
}

#define SAMPLETIME_REGELAAR 0.005

//define states
enum state {HOME, CALIBRATIE, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
uint8_t state = HOME;

// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
const double g = 360; // Aantal graden 1 rotatie
const double c = 4200; // Aantal counts 1 rotatie
const double q = c/(g);

//PI-controller constante
const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
const double motor1_Ki = 0.002; // Integrating gain m1.
const double motor1_Ts = 0.01; // Time step m1
double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1

// Reusable P controller
double Pc (double error, const double Kp)
{
    return motor1_Kp * error;
}

// Measure the error and apply output to the plant
void motor1_controlP()
{
    double referenceP1 = PotMeter1.read();
    double positionP1 = q*motor1.getPosition();
    double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
}

// Reusable PI controller
double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
{
    err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
    return motor1_Kp*error + motor1_Ki*err_int;
} // De totale fout die wordt hersteld met behulp van PI control.

//bool Cali = false;
//double TimeCali = 5;

// Filter1 = High pass filter tot 20 Hz
double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
// Filter2 = Low pass filter na 60 Hz
double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
// Filter3 = Notch filter at 50 Hz
double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;

// Filter4 = Lowpass filter at 5 Hz
double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;

double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7, u8, u9;
double final_filter1;

// Standaard formule voor het biquad filter
double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)

{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2=v1;
    v1=v;
    return y;
}

void Filters()
{
    u1 = EMG.read();
    //Highpass
    y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
    u2 = y1;
    y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
    //Lowpass
    u3 = y2;
    y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
    u4 = y3;
    y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
    // Notch
    u5 = y4;
    y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
    u6 = y5;
    y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
    u7 = y6;
    y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
    y8 = fabs (y7);
    //smooth
    u8 = y8;
    y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
    u9 = y9;
    final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
    
    scope.set (0, y8);
   // scope.set (1, y2);
   // scope.set (2, y4);
   // scope.set (3, y7);
   scope.set (1, final_filter1);
   //scope.set (2, final_filter1); 
    scope.send ();
    } 
 

void motor1_controlPI()
{
    double referencePI1 = PotMeter1.read();
    double positionPI1 = q*motor1.getPosition();
    double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
}

const int pressed = 0;

double H;
double P;
double D;


void gethome(){
    H = motor1.getPosition();
}

void move_motor1_ccw (){
    motor1_direction = 0;
    motor1_speed = 1;
}

void move_motor1_cw (){
    motor1_direction = 1;
    motor1_speed = 1;
}

void movetohome(){
    P = motor1.getPosition();
    
    if (P >= -28 && P <= 28){
        motor1_speed = 0;
    }
    else if (P > 24){
        motor1_direction = 1;
        motor1_speed = 0.1;
    }
    else if (P < -24){
        motor1_direction = 0;
        motor1_speed = 0.1;
    }
}

void move_motor1()
{
        if (final_filter1 > 0.03){
            pc.printf("Moving clockwise \n\r");
            move_motor1_cw ();
            }
        else if (button_2 == pressed){
            pc.printf("Moving counterclockwise \n\r");
            move_motor1_ccw ();
            }
        else {
            pc.printf("Not moving \n\r"); 
            motor1_speed = 0;
            }
}

//void read_encoder1 ()    // aflezen van encoder via hidscope??
//{
//    scope.set(0,motor1.getPosition());
    //led.write(motor1.getPosition()/100.0);
//    scope.send();
//    wait(0.2f);
//}

void print_position(){
    pc.printf("move motor \n\r");
    wait(0.2f);
    }
int main()
{    
    while (true) {
        pc.baud(9600);          //pc baud rate van de computer
        EMG_Control.attach_us(Filters,1e3);
        
    switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
        
        case HOME:      //positie op 0 zetten voor arm 1
         {
            pc.printf("home\n\r");
            //read_encoder1();
            gethome();
            pc.printf("Home-position is %f\n\r", H);
            state = CALIBRATIE;
            wait(5);
            break;
        }
        
        //case CALIBRATIE:
        //{        
            //pc.printf("Aanspannen in 10 \n\r");
            //wait(10);
            //EMG_Control.attach_us(MyController,1e3);
            //Timer_Calibration.start();
            //if (Timer_Calibration < TimeCali) {
            //    Cali = true;
            //    pc.printf("Aanspannen \n\r");
            //} 
            //else {
            //    Cali = false;
            //}  
            //pc.printf("Stoppen \n\r"); 
            //Timer_Calibration.stop();
            //Timer_Calibration.reset();
            //state = MOVE_MOTOR;
          //  break;
        //}
        
        case MOVE_MOTOR:            //motor kan cw en ccw bewegen a.d.h.v. buttons
        {
            pc.printf("move_motor\n\r");
            while (state == MOVE_MOTOR){
            move_motor1();
            print_position();
            if (button_1 == pressed && button_2 == pressed){
            state = BACKTOHOMEPOSITION;
            }
            }
            wait (1);
            break; 
        }
        
        case BACKTOHOMEPOSITION:    //motor gaat terug naar homeposition
        {
            pc.printf("backhomeposition\n\r");
            wait (1);
            
            ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
            //EMG_Control.attach_us(Filters,1e3);

            while(state == BACKTOHOMEPOSITION){
                movetohome();
                while(regelaar_ticker_flag != true);
                regelaar_ticker_flag = false;
                
                pc.printf("pulsmotorposition1 %d", motor1.getPosition());
                pc.printf("referentie %f\n\r", H);
                
                if (P <=24 && P >= -24){
                pc.printf("pulsmotorposition1 %d", motor1.getPosition());
                pc.printf("referentie %f\n\r", H);
                state = STOP;
                pc.printf("Laatste positie %f\n\r", motor1.getPosition());
                break;
                }
            }
        }
        case STOP:
        {
           static int c;
           while(state == STOP){
            motor1_speed = 0;
            if (c++ == 0){
            pc.printf("einde\n\r");
            }
               }
            break;
            }
}
}
}