File content as of revision 15:17de575b7385:

#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
#include "Biquad.h"
#include "controlandadjust.h"

//info out
HIDScope scope(6);
Ticker scope_ticker;
const double scope_frequency=500;
Serial pc(USBTX,USBRX);

DigitalOut ledred(LED1);
DigitalOut ledgreen(LED2);
DigitalOut ledblue(LED3);

////////////////ENCODERS
const float cpr_sensor=32;
const float cpr_shaft=cpr_sensor*131;
QEI encoder1(D12,D13,NC,cpr_sensor);/// maybe use Encoder in stead of QEI, because Encoder had setposition
QEI encoder2(D10,D11,NC,cpr_sensor);
const double PIE=3.14159265359;
const float counttorad=((2*PIE)/cpr_shaft);

/////////////////////////////////CALIBRATION (MODE)
int modecounter=1;
DigitalIn changemodebutton(PTA4);

Ticker readbuttoncalibrate_ticker;
const double readbuttoncalibrate_frequency=10;

Ticker ledblink_ticker;
const double ledblink_frequency=4;

const double radpersec_calibrate=0.1*PIE;

DigitalIn buttonR(D2);
DigitalIn buttonL(D3);



//////////////////////////////////CONTROLLER
controlandadjust mycontroller; // make a controller
//controller constants
float Kp=0.5;
float Ki=0.01;
float Kd=0.001;
Ticker control_ticker;
const double control_frequency=25;
const double Ts_control=1.0/control_frequency;

float error1_int=0;// storage variables for the errors
float error2_int=0;
float error1_prev=0;
float error2_prev=0;

InterruptIn valuechangebutton(PTC6);//button to change controller constants

//safetyandthreshold
AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person

Ticker safetyandthreshold_ticker; // ticker to read potmeters
const double safetyandthreshold_frequency=1; // frequency for the ticker

float threshold_value=1;//initial threshold value

////////////////////////////////FILTER
#include "filtervalues.h"
Ticker filter_ticker;
const double filter_frequency=500;
Biquad myfilter1;
Biquad myfilter2;

AnalogIn emg1_input(A0);
AnalogIn emg2_input(A1);

double filteredsignal1=0;
double filteredsignal2=0;
float filter_extragain=1;

/////////////////READSIGNAL
Ticker readsignal_ticker;
const double readsignal_frequency=25;

DigitalOut led1(PTC12);
DigitalOut led2(D9);

//////////////////////////////// POSITION AND ANGLE SHIZZLE
float desired_position=0;
float desired_angle[]= {0,0};
float mm_per_sec_emg=50;// move the pod 50 mm per sec if muscle is flexed
float fieldwidth=473;
float safetymarginfield=75; //adjustable, tweak for maximum but safe range
float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
float rad_per_sec_emg=0.25*PIE;// THIS ONE IS NOT NESSECARY FOR ACTUAL PROGRAM

//////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
volatile bool scopedata_go=false,
              control_go=false,
              filter_go=false,
              safetyandthreshold_go=false,
              readsignal_go=false,
              switchedmode=true,
              readbuttoncalibrate_go=false,
              ledblink_go=false;

void scopedata_activate()
{
    scopedata_go=true;
}
void control_activate()
{
    control_go=true;
}
void filter_activate()
{
    filter_go=true;
}
void safetyandthreshold_activate()
{
    safetyandthreshold_go=true;
}
void readsignal_activate()
{
    readsignal_go=true;
}
void readbuttoncalibrate_activate()
{
    readbuttoncalibrate_go=true;
}
void ledblink_activate()
{
    ledblink_go=true;
}

////////////////////////FUNCTIONS
//gather data and send to scope
void scopedata()
{
    scope.set(0,desired_angle[0]);
    scope.set(1,counttorad*encoder1.getPulses());
    scope.set(2,mycontroller.motor1pwm());
    scope.set(3,desired_angle[1]);
    scope.set(4,counttorad*encoder2.getPulses());
    scope.set(5,filteredsignal1);
    scope.send();
}
//read potmeters and adjust the safetyfactor and threshold
void safetyandthreshold()
{
    mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
    threshold_value=((ceil (10*threshold_pot.read()) )/10); // adjust the threshold value between 0 and 1 rounded to 1 decimal
}
/////filter
void filtereverything()
{
    //filter_timer.reset();
    // filter_timer.start();
    //pass1 so f1
    double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
    double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);

    //pass2 so f2
    double pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
    double pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);

    //pass3 so f3
    double pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
    double pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);

    //pass4 so f4
    double pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
    double pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);

    //pass5 so f5
    double pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
    double pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);

    ///// take absolute value
    double pass5_emg1_abs=(fabs(pass5_emg1));
    double pass5_emg2_abs=(fabs(pass5_emg2));

    //pass6 so f6
    double pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
    double pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);


    //pass7 so f7
    double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
    double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);

    filteredsignal1=(pass7_emg1*9e11*filter_extragain);
    filteredsignal2=(pass7_emg2*9e11*filter_extragain);

    //filter_timer.stop();
}



//adjust controller values when sw2 is pressed
void valuechange()
{
    mycontroller.STOP();
    pc.printf("KP is now %f, enter new value\n",Kp);
    pc.scanf("%f", &Kp);

    pc.printf("KI is now %f, enter new value\n",Ki);
    pc.scanf("%f", &Ki);

    pc.printf("KD is now %f, enter new value\n",Kd);
    pc.scanf("%f", &Kd);

    pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
    pc.scanf("%f", &filter_extragain);
}



const float schiethoek=0.35*PIE;
const float schiettijd=0.5;
void shoot() // THIS NEEDS ADJUSTMEND
{
    pc.printf("SHOOT\n");
    //hoeken groter maken
    desired_angle[0]-=schiethoek;
    desired_angle[1]+=schiethoek;

    Timer schiettimer;
    schiettimer.reset();
    schiettimer.start();
    float pass=0;
    while(schiettimer.read()<=schiettijd) {
// errors berekenen en naar de controller passen
        float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
        float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
        mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int, error2_int);
        scopedata();
        wait (Ts_control-(schiettimer.read()-Ts_control*pass)); // even wachten anders wordt de while loop te snel doorlopen en gaan de motoren wak
        ledblue=!ledblue;
        pass++;
    }
    schiettimer.stop();
    ledblue=0;
    //terug na schieten
    desired_angle[0]+=schiethoek;
    desired_angle[1]-=schiethoek;
}

void readsignal()
{
    //check if pod has to shoot
    if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
        led1=led2=1;
        shoot();
        // check if pod has to move to the right
    } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
        led1=1;
        led2=0;
        desired_position += (rad_per_sec_emg/readsignal_frequency);// move desiredposition right ADJUS TO MM IN LAST VERSEION
        if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
            desired_position=maxdisplacement;
        } else {
            desired_position=desired_position;
        }
        // check if pod has to move to the left
    } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
        led1=0;
        led2=1;
        desired_position -= (rad_per_sec_emg/readsignal_frequency);//move desiredposition left ADJUST TO MM IN FINAL VERSION
        if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
            desired_position=(-1*maxdisplacement);
        } else {
            desired_position=desired_position;
        }
    } else {
        led1=led2=0;
    }
    desired_angle[0]=(desired_position);// REMOVE IN FINAL VERSION
    desired_angle[1]=desired_position;//REMOVE IN FINAL VERSION
}

void changemode()
{
    mycontroller.STOP();
    switchedmode=true;
    modecounter++;
    if (modecounter==4) {
        modecounter=0;
    } else {
        modecounter=modecounter;
    }
    wait(1);
}



int main()
{
    //tickers
    safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
    filter_ticker.attach(&filter_activate,1.0/filter_frequency);
    control_ticker.attach(&control_activate,1.0/control_frequency);
    scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
    readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
    readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
    ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);

    ledred=0;
    ledgreen=0;
    ledblue=0;

    while(1) {
        if (changemodebutton==0) {
            changemode();
        }
        if (scopedata_go==true) {
            scopedata();
            scopedata_go=false;
        }
        if (safetyandthreshold_go==true) {
            safetyandthreshold();
            safetyandthreshold_go=false;
        }
        ///////////////////////////////////////////NORMAL RUNNING MODE
        if(modecounter==0) {
            if (switchedmode==true) {
                encoder1.reset();
                encoder2.reset();
                pc.printf("Program running\n");//
                ledgreen=1;
                led1=led2=ledred=0;
                switchedmode=false;
            }

            if (filter_go==true) {
                filtereverything();
                filter_go=false;
            }
            if (control_go==true) {
                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
                float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                control_go=false;
            }
            if (readsignal_go==true) {
                readsignal();
                readsignal_go=false;
            }
            valuechangebutton.fall(&valuechange);
        }
        ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
        if (modecounter==1) {
            if(switchedmode==true) {
                pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
                switchedmode=false;
                ledred=1;
                led1=led2=ledgreen=0;
            }
            if (ledblink_go==true) {
                led1=!led1;
            }
            if (control_go==true) {
                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
                float error2=0;// this is the error you want to use
                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                control_go=false;
            }
            if (readbuttoncalibrate_go==true) {
                if (buttonR.read()==0 && buttonL.read()==1) {
                    desired_angle[0] += (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
                if (buttonR.read()==1 && buttonL.read()==0) {
                    desired_angle[0] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
            }
        }
        ////////////////////////////////////////////CALIBRATE LEFT ARM
        if (modecounter==2) {
            if(switchedmode==true) {
                pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
                ledred=1;
                led1=led2=ledgreen=0;
                switchedmode=false;
            }
            if (ledblink_go==true) {
                led2=!led2;
            }
            if (control_go==true) {
                float error1=0;
                float error2=(desired_angle[1]-counttorad*encoder2.getPulses());// this is the error you want to use
                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                control_go=false;
            }
            if (readbuttoncalibrate_go==true) {
                if (buttonR.read()==0 && buttonL.read()==1) {
                    desired_angle[1] += (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
                if (buttonR.read()==1 && buttonL.read()==0) {
                    desired_angle[1] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
            }
        }
        if (modecounter==3) {
            if (ledblink==true) {
                ledgreen=!ledgreen;
                //buttoncontrolmode
            }

        }
    }
}