File content as of revision 2:91bf9f1765ef:

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

//////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
//info uit
HIDScope scope(5);
Serial pc(USBTX, USBRX);

//encoders
QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
QEI encoder2 (D10,D11,NC,32);

//ingangen
DigitalIn butR(D2);
DigitalIn butL(D3);
InterruptIn changecontrollervaluesbutton(PTA4);//button to adjus controllervalues

//frequencies
const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
const double control_frequency=25;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
const double read_but_frequency=25;

//tickers
Ticker hidscope_ticker;
Ticker control_ticker;
Ticker read_but_ticker;
Timeout schieten_timeout;

// controller values and errors
controlandadjust mycontroller;
const float Kp=0.5;
const float Ki=0.01;
const float Kd=0.001;
const float Ts_control=1.0/control_frequency;
float error1=0;
float error2=0;
float error1_int=0;
float error2_int=0;
float error1_prev=0;
float error2_prec=0;

//constants
const float cpr=32*131;
const float PI=3.1415;
const float counttorad=((2*PI)/cpr);
const float radpersecbut=(PI/read_but_frequency);
const float schiethoek=0.5*PI;
const float schiettijd=0.5;

//angle for motor
float desiredangle[]= {0,0};
float desiredposition=0;
////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
//go flags
volatile bool scopedata_go=false, control_go=false,read_but_go=false;

//acvitator functions

void scopedata_activate()
{
    scopedata_go=true;
}
void control_activate()
{
    control_go=true;
}
void read_but_activate()
{
    read_but_go=true;
}
///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////

//scopedata
void scopedata()
{
    scope.set(0,desiredposition);//gewenste hoek van arm
    scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft
    scope.set(2,mycontroller.motor1pwm());//pwm signaal naar motor toe
    scope.set(3,counttorad*encoder2.getPulses());//hoek in rad van outputshaft
    scope.set(4,mycontroller.motor2pwm());//pwm signaal naar motor toe
    scope.send();
}



void schieten()
{
    pc.printf("SHOOT\n");
    //hoeken groter maken
    desiredangle[0]-=schiethoek;
    desiredangle[1]+=schiethoek;

    Timer schiettimer;
    schiettimer.reset();
    schiettimer.start();
    while(schiettimer<=schiettijd) {
        error1=(desiredangle[0]-counttorad*encoder1.getPulses());
        error2=(desiredangle[1]-counttorad*encoder2.getPulses());
        mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int, error2_int);
    }
    schiettimer.stop();

    //terug na schieten
    desiredangle[0]+=schiethoek;
    desiredangle[1]-=schiethoek;
}

//////////////////////////////////////////////////MAIN///////////////////////////////////
int main()
{
    //tickers
    hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
    control_ticker.attach(&control_activate,1.0/control_frequency);
    read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);

    while(1) {
        //read buttons and adjust angles
        if (read_but_go==true) {
            float buttonR=butR.read(), buttonL=butL.read();
            if (buttonR==0 && buttonL ==1) {
                desiredposition += (radpersecbut);
                read_but_go=false;
            }
            if (buttonR==1 && buttonL==0) {
                desiredposition -= (radpersecbut);
                read_but_go=false;
            }
            //schieten
            if (buttonR==0 && buttonL==0) {
                schieten();
                read_but_go==false;
            } else {
                desiredposition=desiredposition;
            }
            desiredangle[0]=desiredposition;
            desiredangle[1]=desiredposition;

            read_but_go=false;
        }
//control motors with pi controller
        if (control_go==true) {
            error1=(desiredangle[0]-counttorad*encoder1.getPulses());
            error2=(desiredangle[1]-counttorad*encoder2.getPulses());
            mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int,error2_int);
            control_go=false;
        }
        //call scopedata
        if (scopedata_go==true) {
            scopedata();
            scopedata_go=false;
        }
        //unit om controllervalues aan te passen
        if (changecontrollervaluesbutton.read()==0) {
            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);
        }
    }
    return 0;
}