File content as of revision 0:24628832f21d:

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

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

//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);

//uitgangen
DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
PwmOut motor2_speed_control(D5);
DigitalOut motor2_direction(D4);
const int CW=1; //clockwise
const int CCW=0; //counterclockwise

//frequencies
const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
const double pi_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 pi_control_ticker;
Ticker read_but_ticker;

//constants
const float cpr=32*131;
const float PI=3.1415;
const float counttorad=((2*PI)/cpr);
const float counttodeg=(360/cpr);
const float mmpersecbut=(30/(read_but_frequency));
const float radpersecbut=(PI/read_but_frequency);
const float armlength=400;//length "upper arm" in mm

///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////

//DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!


double desiredangle[2]= {0,0};
double desiredposition=0;
double signal_motor[2]= {0,0};
////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
//go flags
volatile bool scopedata_go=false, pi_control_go=false,read_but_go=false;

//acvitator functions

void scopedata_activate()
{
    scopedata_go=true;
}
void pi_control_activate()
{
    pi_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,counttodeg*encoder1.getPulses());//hoek in rad van outputshaft
    scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
    scope.set(3,counttodeg*encoder2.getPulses());//hoek in rad van outputshaft
    scope.set(4,motor2_speed_control.read());//pwm signaal naar motor toe
    scope.send();
}
//////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
float pi_kp=0.5;
float pi_ki=0.01;
float Ts=1.0/pi_control_frequency;
double e[2]= {0,0};
double motor_error_int[2]= {0,0};

// Reusable PI controller
void pi_control()
{
    motor_error_int[0] = motor_error_int[0] + Ts * e[0]; // e_int is changed globally because it’s ’by reference’ (&)
    signal_motor[0]=pi_kp*e[0]+pi_ki*motor_error_int[0];

    motor_error_int[1] = motor_error_int[1] + Ts * e[1]; // e_int is changed globally because it’s ’by reference’ (&)
    signal_motor[1]=pi_kp*e[1]+pi_ki*motor_error_int[1];
}

//////////////////////////////////////////////////MAIN///////////////////////////////////
int main()
{
    //set initial shizzle
    motor1_speed_control.write(0);
    motor2_speed_control.write(0);

    //tickers
    hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
    pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
    read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);

    while(1) {
        //control motor 1 with a pi controller

        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;
            } else {
                desiredposition=desiredposition;
            }
            desiredangle[0]=desiredposition;
            desiredangle[1]=desiredposition;
            read_but_go=false;
        }

        if (pi_control_go==true) {
            e[0]=desiredangle[0]-(counttorad*encoder1.getPulses());
            e[1]=desiredangle[1]-(counttorad*encoder2.getPulses());
            pi_control();
            //motor1
            if (signal_motor[0]>=0) {//determine CW or CCW rotation
                motor1_direction.write(CW);
            } else {
                motor1_direction.write(CCW);
            }

            if (fabs(signal_motor[0])>=1) { //check if signal is <1
                signal_motor[0]=1;//if signal >1 make it 1 to not damage motor
            } else {
                signal_motor[0]=fabs(signal_motor[0]);// if signal<1 use signal
            }

            //motor 2
            if (signal_motor[1]>=0) {//determine CW or CCW rotation
                motor2_direction.write(CW);
            } else {
                motor2_direction.write(CCW);
            }

            if (fabs(signal_motor[1])>=1) { //check if signal is <1
                signal_motor[1]=1;//if signal >1 make it 1 to not damage motor
            } else {
                signal_motor[1]=fabs(signal_motor[1]);// if signal<1 use signal
            }



            motor1_speed_control.write(fabs(signal_motor[0]));//write signal to motor
            motor2_speed_control.write(fabs(signal_motor[1]));//write signal to motor

            pi_control_go=false;
        }
        //call scopedata
        if (scopedata_go==true) {
            scopedata();
            scopedata_go=false;
        }
    }
    return 0;
}