File content as of revision 2:5093b66c9b26:

#include <math.h>
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "FastPWM.h"
#include "encoder.h"

const double PI = 3.1415926535897;  

InterruptIn but1(D2);
InterruptIn but2(D3);
AnalogIn potMeter1(A2);
AnalogIn potMeter2(A1);

DigitalOut motor1_direction(D4);                                                //value 0=CCW, 1=CW
FastPWM motor1_pwm(D5);                                                          //Biorobotics Motor 1 PWM controle van de snelheid
DigitalOut motor2_direction(D7);                                                //value 0=CCW, 1=CW
FastPWM motor2_pwm(D6);

Ticker loop_ticker; 
// SERIAL COMMUNICATION WITH PC.................................................
Serial pc(USBTX, USBRX);

enum States {s_idle, s_cali_enc, s_demo};
States state;

// ENCODER .....................................................................
Encoder enc1 (D13, D12, true); //encoder 1 gebruiken
Encoder enc2 (D9,  D8,  true); //encoder 2 gebruiken 
        
float dt = 0.001;
double e_int = 0;
double e_int2 = 0;
float theta1;
float theta2;
int enc1_zero = 0;//the zero position of the encoders, to be determined from the
int enc2_zero = 0;//encoder calibration
int enc1_value;
int enc2_value;
bool state_changed = false; 
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
volatile bool failure_occurred = false;

void do_nothing()
    //Idle state. Used in the beginning, before the calibration states.  
{
    if (but1_pressed) {
        state_changed = true;
        state = s_cali_enc;
        but1_pressed = false;
    }
}

void cali_enc()
{
    if (state_changed) {
        pc.printf("Started encoder calibration\r\n");
        state_changed = false;
    }
    if (but1_pressed) {
        pc.printf("Encoder has been calibrated \r\n");
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        but1_pressed = false;
        state_changed = true;
        state = s_demo;
        pc.printf("enc01: %i\r\n", enc1_zero);
        pc.printf("enc1value: %i\r\n", enc1_value);
        pc.printf("enc02: %i\r\n",enc2_zero);
        pc.printf("enc2value: %i\r\n", enc2_value);
        pc.printf("hoek 1: %f\r\n",theta1);
        pc.printf("hoek 2: %f\r\n",theta2);
        
 
    }
}
    
double GetReferencePosition() 
{
    double Potmeterwaarde = potMeter1.read(); //naam moet universeel worden
    int maxwaarde = 200;                   // = 64x64
    double refP = Potmeterwaarde*maxwaarde;
    return refP;                            // value between 0 and 4096 
}
 
double GetReferencePosition2() 
{
    double potmeterwaarde2 = potMeter2.read();
    int maxwaarde2 = 200;                   // = 64x64
    double refP2 = potmeterwaarde2*maxwaarde2;
    return refP2;                            // value between 0 and 4096 
}
   
double FeedBackControl(double error, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
    double kp = 0.0015;                               // kind of scaled.
    double Proportional= kp*error;
    
    double ki = 0.0001;                           // kind of scaled.
    e_int = e_int+dt*error;
    double Integrator = ki*e_int;
    
    
    double motorValue = Proportional + Integrator ;
    return motorValue;
}
 
double FeedBackControl2(double error2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
    double kp2 = 0.002;                             // kind of scaled.
    double Proportional2= kp2*error2;
    
    double ki2 = 0.00005;                           // kind of scaled.
    e_int2 = e_int2+dt*error2;
    double Integrator2 = ki2*e_int2;
    
    
    double motorValue2 = Proportional2 + Integrator2 ;
    return motorValue2;
    
}
 
void SetMotor1(double motorValue)
{
    if (motorValue >= 0) {
        motor1_direction = 0;
    }
    else {
        motor1_direction = 1;
    }
 
    if  (fabs(motorValue) > 1) {
        motor1_pwm = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
    }
    else {    
        motor1_pwm = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
    }
}
 
void SetMotor2(double motorValue2)
{
    if (motorValue2 >= 0) {
        motor2_direction = 1;
    }
    else {
        motor2_direction = 0;
    }
 
    if  (fabs(motorValue2) > 1) {
        motor2_pwm = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
    }
    else {    
        motor2_pwm = fabs(motorValue2);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
    }
}
 
void MeasureAndControl(void)
{
    //SetpointRobot(); 
    // RKI aanroepen
    //RKI();
    // hier the control of the 1st control system
    double refP = GetReferencePosition();                    //KOMT UIT RKI
    double Huidigepositie = enc1.getPosition(); 
    double error = (refP - Huidigepositie);// make an error
    double motorValue = FeedBackControl(error, e_int);
    SetMotor1(motorValue);
    // hier the control of the 2nd control system
    double refP2 = GetReferencePosition2(); 
    double Huidigepositie2 = enc2.getPosition(); 
    double error2 = (refP2 - Huidigepositie2);// make an error
    double motorValue2 = FeedBackControl2(error2, e_int2);
    SetMotor2(motorValue2);
    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
}
    
void measure_signals()
{
    enc1_value = enc1.getPosition();
    enc2_value = enc2.getPosition();
    enc1_value -= enc1_zero;
    enc2_value -= enc2_zero;
    theta1 = (float)(enc1_value)/(float)(8400)*2*PI;
    theta2 = (float)(enc2_value)/(float)(8400)*2*PI;    
  
    }  
  
void state_machine()
{
    //run current state
    switch (state) {
        case s_idle:
            do_nothing();
            break;
        case s_cali_enc:
            cali_enc();
            break;
        case s_demo:
        MeasureAndControl();
        break;
        
}
}

void but1_interrupt()
{
    if(but2.read()) {//both buttons are pressed
        failure_occurred = true;
    }
    but1_pressed = true;
    pc.printf("Button 1 pressed \n\r");
}
 
void but2_interrupt()
{
    if(but1.read()) {//both buttons are pressed
        failure_occurred = true;
    }
    but2_pressed = true;
    pc.printf("Button 2 pressed \n\r");
}

void main_loop()
{
    measure_signals();
    state_machine();
}

int main()
{
    pc.baud(115200);
    pc.printf("Executing main()... \r\n");
    state = s_idle;
    
    
    but1.fall(&but1_interrupt);
    but2.fall(&but2_interrupt);
    loop_ticker.attach(&main_loop, dt);
    pc.printf("main_loop is running\n\r");
   
  }