File content as of revision 2:d0076e9d0a7f:

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

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

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

//ingangen
AnalogIn pot1(A2);

//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 float pwm_frequency=1000;
const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
const double pi_control_frequency=5;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!

//tickers
Ticker hidscope_ticker;
Ticker pi_control_ticker;

//constants
const int cpr=32*131;
const float PI=3.1415;
const float counttorad=((2*PI)/cpr);

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

//DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
const float motor1_pi_kp=0.5;
const double motor1_pi_ki=0.01;
double motor1_error_int=0;
////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
//go flags
volatile bool scopedata_go=false, pi_control_go=false;

//acvitator functions

void scopedata_activate()
{
    scopedata_go=true;
}
void pi_control_activate()
{
    pi_control_go=true;
}
///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////

//scopedata
void scopedata()
{
    scope.set(0,2*PI*pot1.read());//gewenste hoek in rad van potmeter
    scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft
    scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
    scope.send();
}
//////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
// Reusable PI controller
double pi_control( double e, const double Kp, const double Ki, double Ts, double& motor1_error_int)
{
    motor1_error_int = motor1_error_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
    return Kp*e+Ki*motor1_error_int;
}

//////////////////////////////////////////////////MAIN///////////////////////////////////
int main()
{
    //set initial shizzle
    //motor1_speed_control.period(1.0/pwm_frequency);
    motor1_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);

    while(1) {
        //control motor 1 with a pi controller
        if (pi_control_go==true) {
            double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
            double signal_motor1=pi_control(error,motor1_pi_kp,motor1_pi_ki,
                                            1/pi_control_frequency,motor1_error_int);//send error to p controller
            if (signal_motor1>=0) {//determine CW or CCW rotation
                motor1_direction.write(CW);
            } else {
                motor1_direction.write(CCW);
            }

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

            motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
            pi_control_go=false;
        }
        //call scopedata
        if (scopedata_go==true) {
            scopedata();
            scopedata_go=false;
        }
    }
    return 0;
}