File content as of revision 5:810892d669f9:

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

const double PI = 3.1415926535897;  

InterruptIn but1(D1);
InterruptIn but2(D0);
DigitalIn butsw2(SW3);
DigitalIn butsw3(SW2);
AnalogIn potMeter1(A5);
AnalogIn potMeter2(A4);

DigitalOut motor1_direction(D4);                                                //richting van motor1
FastPWM motor1_pwm(D5);                                                         //Motor 1 PWM controle van de snelheid
DigitalOut motor2_direction(D7);                                                //richting van motor2
FastPWM motor2_pwm(D6);                                                         //Motor 2 PWM controle van de snelheid

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

enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibratie};
States state;

// ENCODER .....................................................................
QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
QEI enc2 (D9,  D8,  NC, 4200); //encoder 2 gebruiken 
        
const float dt = 0.001;
double e_int = 0;
double e_int2 = 0;
double theta1;
double theta2;
int enc1_zero = 0;
int enc2_zero = 0;
int enc1_value;
int enc2_value;
const int maxwaarde = 400;

volatile double Huidigepositie1=0;
volatile double Huidigepositie2=0;
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;

// RKI VARIABELEN............................................................... 
// Lengtes zijn in meters
// Vaste variabelen:
const double L0 = 0.30;                                                         // lengte arm 1 --> moet nog goed worden opgemeten!
const double L2 = 0.30;                                                         // Lengte arm 2 --> moet ook nog goed opgemeten worden!
const double r_trans = 0.035;                                                   // gebruiken voor translation naar shaft rotation 

// Variërende variabelen: 
double q1 = 0;                                                                  // Motorhoek van joint 1 op beginpositie
double q2 = PI/2;                                                               // Motorhoek van joint 2 op beginpositie
double v_x;                                                                     // Snelheid van end effector in x richting --> Door EMG signals
double v_y;                                                                     // Snelheid van end effector in y richting --> Door EMG signals

double Lq1;                                                                     // Translatieafstand als gevolg van motor rotation joint 1
double Cq2;                                                                     // Joint angle van het systeem (gecorrigeerd voor gear ratio 1:1.1)

double q1_dot;                                                                  // Benodigde hoeksnelheid van motor 1 
double q2_dot;                                                                  // Benodigde hoeksnelheid van motor 2  

double q1_ii;                                                                   // Referentie signaal voor motorhoek q1 
double q2_ii;                                                                   // Referentie signaal voor motorhoek q2 

//PI VARIABELEN................................................................
const double kp=0.002;                                                          // Soort van geschaald --> meer onderzoek nodig
const double ki=0.0001;
const double kp2=0.002;
const double ki2=0.0001;

void rest()
                                                                                // Rust. Hier wordt niets uitgevoerd. Wanneer button1 
{                                                                               // wordt ingedrukt gaan we naar de volgende state waar
    if (but1_pressed) {                                                         // de encoders worden gekalibreerd.
        state_changed = true;
        state = s_calibratie_encoder;
        but1_pressed = false;
    }
}

void calibratie_encoder()                                                       // calibratie encoder. Hier worden de encoders gekalibreerd en op 
{                                                                               // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de 
    if (state_changed) {                                                        // demo modus, wanneer op button 2 wordt gedruk gaan we naar    
        pc.printf("Started encoder calibration\r\n");                           // de EMG calibratie
        state_changed = false;
    }
    if (but1_pressed) {
        pc.printf("Encoder has been calibrated \r\n");
        enc1_value = enc1.getPulses();
        enc2_value = enc2.getPulses();
        //enc1_value -= enc1_zero;
        //enc2_value -= enc2_zero;
        theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
        theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        but1_pressed = false;
        state_changed = true;
        state = s_demonstratie;
        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) {
        pc.printf("Encoder has been calibrated \r\n");
        enc1_value = enc1.getPulses();
        enc2_value = enc2.getPulses();
        //enc1_value -= enc1_zero;
        //enc2_value -= enc2_zero;
        theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
        theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        but2_pressed = false;
        state_changed = true;
        state = s_EMGcalibratie;
        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);
    }
}

void demonstratie()
{
    if (state_changed) {                                                        //   
        pc.printf("Demonstratie gestart\r\n")   ;                                //
        state_changed = false;
    }
  }  
void EMGcalibratie()
{    

}

void RKI()
{    
        Lq1 = q1*r_trans;                            
        Cq2 = q2/1.1;                               //1.1 is gear ratio

        q1_dot = (v_x + (v_y*(/*L1+*/L2*sin(q2/1.1)))/(L0 + q1*r_trans + L2*cos(q2/1.1)))/r_trans;     
        q2_dot = (1.1*v_y)/(L0 + q1*r_trans + L2*cos(q2/1.1));                                       

        q1_ii = q1 + q1_dot*dt;                       
        q2_ii = q2 + q2_dot*dt; 
        
        q1 = q1_ii;
        q2 = q2_ii;   
                           
}

double GetReferencePosition() 
{
    double Potmeterwaarde = potMeter2.read(); 
                        
    double refP = Potmeterwaarde*maxwaarde;
    return refP;                              
}
 
double GetReferencePosition2() 
{
    double potmeterwaarde2 = potMeter1.read();                   
    double refP2 = potmeterwaarde2*maxwaarde;
    return refP2;                            
}
  
double FeedBackControl(double error, double &e_int)   
{
    double Proportional= kp*error1;
                              
    e_int = e_int+dt*error1;
    double Integrator = ki*e_int;
    
    motorValue1 = Proportional + Integrator ;
    return motorValue1;
}
 
double FeedBackControl2(double error2, double &e_int2)   
{                            
    double Proportional2= kp2*error2;
                              
    e_int2 = e_int2+dt*error2;
    double Integrator2 = ki2*e_int2;
    
    double motorValue2 = Proportional2 + Integrator2 ;
    return motorValue2;
}


void SetMotor1(float motorValue1) {
    // gegeven motorValue1 <=1, schrijft snelheid naar pwm.
    // MotorValues buiten range worden afgekapt dat ze binnen de range vallen.
    motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
}

void SetMotor2(float motorValue2) {
    motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
}    
 
void MeasureAndControl()
{
    // RKI aanroepen
    //RKI();
    // hier the control of the 1st control system
    refP = GetReferencePosition();                    //moet eigenlijk nog met RKI
    Huidigepositie1 = enc1.getPulses(); 
    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.getPulses(); 
    error2 = (refP2 - Huidigepositie2);// make an error
    motorValue2 = FeedBackControl2(error2, e_int2);
    SetMotor2(motorValue2);
    pc.printf("refP = %d, huidigepos = %d, motorvalue = %d, refP2 = %d, huidigepos2 = %d, motorvalue2 = %d \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
        
}

void direction()
{ 
    if (butsw2==0) {
        if (A==true){// zodat het knopje 1 x wordt afgelezen
        // richting veranderen
        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 state_machine()
{
    //run current state
    switch (state) {
        case s_idle:                                                            // in deze state gebeurd niets. Als op knop 1 wordt gedrukt
            rest();                                                             // gaan we over naar s_calibratie_encoder
            break;
        case s_calibratie_encoder:                                              // in deze state worden de encoders gekalibreerd. knop 1 -> s_demonstratie
            calibratie_encoder();                                               // knop 2 -> s_EMGcalibratie
            break;
        case s_demonstratie:                                                    // in deze state kunnen de motors worden bestuurd met de potmeters
            MeasureAndControl();                                                // en switch 2 en 3( pot1,sw2->motor1 / pot2,sw3->motor2
            direction();                                                        // als op knop 2 wordt gedrukt komen we in de s_idle state
            if (but2_pressed) {
            pc.printf("fail. \r\n");
            but2_pressed = false;
            state_changed = true;
            state = s_idle;
            }
            break;
        case s_EMGcalibratie:
            rest();
            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()
{
    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");
     
}