File content as of revision 3:16df793da2be:

#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(D1);
InterruptIn but2(D0);
DigitalIn butsw2(SW2);
DigitalIn butsw3(SW3);
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_calibratie_encoder, 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;
volatile double Huidigepositie1;
volatile double Huidigepositie2;
volatile double motorValue1;
volatile double motorValue2;
volatile double refP;
volatile double refP2;
volatile double error1;
volatile double error2;
bool state_changed = false; 
volatile bool A=true;
volatile bool B=true;
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
volatile bool butsw2_pressed = false;
volatile bool butsw3_pressed = false;
volatile bool failure_occurred = false;

const bool clockwise = true;
volatile bool direction1 = clockwise;
volatile bool direction2 = clockwise;

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

void calibratie_encoder()
{
    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_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;
        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);
        }
        
        if (but2_pressed) {
        state_changed = true;
        state = s_idle;
        but2_pressed = false;
        pc.printf("FAILURE!\r\n");
        state_changed = false;
    }
}

double GetReferencePosition() 
{
    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
    int maxwaarde = 400;                   // = 64x64
    double refP = Potmeterwaarde*maxwaarde;
    return refP;                            // value between 0 and 4096 
}
 
double GetReferencePosition2() 
{
    double potmeterwaarde2 = potMeter1.read();
    int maxwaarde2 = 400;                   // = 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*error1;
    
    double ki = 0.0001;                           // kind of scaled.
    e_int = e_int+dt*error1;
    double Integrator = ki*e_int;
    
    motorValue1 = Proportional + Integrator ;
    return motorValue1;
}
 
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(float motorValue1) {
    // Given motorValue1<=1, writes the velocity to the pwm control.
    // MotorValues outside range are truncated to within range.
    motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
}

void SetMotor2(float motorValue2) {
    // Given motorValue2<=1, writes the velocity to the pwm control.
    // MotorValues outside range are truncated to within range.
    motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
}    
 
void MeasureAndControl(void)
{
    //SetpointRobot(); 
    // RKI aanroepen
    //RKI();
    // hier the control of the 1st control system
    refP = GetReferencePosition();                    //moet nog met RKI
    Huidigepositie1 = enc1.getPosition(); 
    error1 = (refP - Huidigepositie1);// make an error
    motorValue1 = FeedBackControl(error1, e_int);
    SetMotor1(motorValue1);
    // hier the control of the 2nd control system
    refP2 = GetReferencePosition2(); 
    Huidigepositie2 = enc2.getPosition(); 
    error2 = (refP2 - Huidigepositie2);// make an error
    motorValue2 = FeedBackControl2(error2, e_int2);
    SetMotor2(motorValue2);
    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
        
}

void direction()
{ 
    if (butsw2==0) {
        if (A==true){// zodat het knopje 1 x wordt afgelezen
        // reverses the direction
        motor1_direction.write(direction1 = !direction1);
        pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
        A=false;
        }
    }
    else{
    A=true;
    }
    
    if (butsw3==0){
        if (B==true){
        motor2_direction.write(direction2 = !direction2);
        pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
        B=false;
        }
    }
    else{
    B=true;
}
}

/*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:
            rest();
            break;
        case s_calibratie_encoder:
            calibratie_encoder();
            break;
        case s_demo:
            MeasureAndControl();
            direction();
        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");
   
  }