File content as of revision 4:61d9325d7f2e:

#include "mbed.h"
#include "QEI.h"  //bieb voor encoderfuncties in c++
#include "MODSERIAL.h" //bieb voor modserial
#include "BiQuad.h"
#include "HIDScope.h"
#include "math.h"

InterruptIn sw3(SW3);
DigitalIn sw2(SW2);
DigitalIn encoder1A(D13);
DigitalIn encoder1B(D12);
DigitalIn encoder2A(D11); 
DigitalIn encoder2B(D10);
DigitalIn button_cw(D9);
DigitalIn button_ccw(PTC12);
MODSERIAL pc(USBTX, USBRX);
DigitalOut richting_motor1(D4);
PwmOut pwm_motor1(D5);
DigitalOut richting_motor2(D7);
PwmOut pwm_motor2(D6);
BiQuad PID_controller;
BiQuad PID_controller2;     // maakt een biquad aan voor PID controler 2
//HIDScope Scope(1);

//constanten voor de encoder
const int CW = 2.5; //definitie rechtsom 'lage waarde'
const int CCW =-1; //definitie linksom 'hoge waarde'
const float gearboxratio=131.25; // gearboxratio van encoder naar motor
const float rev_rond=64.0;        // aantal revoluties per omgang van de encoder

volatile double current_position_motor1 = 0;                // current position is 0 op het begin
volatile double previous_position_motor1 = 0;               // previous position is 0 op het begin                    
volatile double current_position_motor2 = 0;                
volatile double previous_position_motor2 = 0;                                   
volatile bool tickerflag;                           //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
volatile double snelheid_motor1;                    // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
volatile double snelheid_motor2;                    // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
double ticker_TS=0.001;                              // zorgt voor een tijdstap van de ticker van 0.1 seconde
volatile double timepassed=0;                       //de waarde van hoeveel tijd er verstreken is
Ticker t;                                           // maakt ticker t aan
volatile double value1_resetbutton = 0;             // deze value wordt gebruikt zodat als er bij de reset button na het bereiken van de waarde nul. De motor stopt met draaien.
volatile double value2_resetbutton = 0;


volatile float d_ref = 0;
volatile float d_ref2 = 0;
volatile float d_ref_new;
const float w_ref = 3;
const float w_ref2 = 4.5;
const double Ts = 0.001;
const double Kp = 0.3823;
const double Ki = 0.127875;
const double Kd = 0.2519; 
const double N = 100;
const double Ts2 = 0.001;
const double Kp2 = 0.3823;
const double Ki2 = 0.127875;
const double Kd2 = 0.2519; 
const double N2 = 100;
Ticker tick;
Ticker controllerticker; 
Ticker tick2;                       // ticker voor reverence voor motor 2
Ticker controllerticker2;            //ticker voor controler voor motor 2
float rev_counts_motor1_rad;
float rev_counts_motor2_rad;
volatile float voltage_motor1=0.5;
volatile float voltage_motor2=0.5;  // pwm motor 2 aansturing
volatile double error1;
volatile double error2;             // error voor tweede motor
volatile double ctrlOutput1;
volatile double ctrlOutput2;        // control output voor motor 2
volatile double d_ref_const_cw = 0;
volatile double d_ref_const_ccw = 0;
volatile int n = 0;

void SwitchN() {                        // maakt simpele functie die 1 bij n optelt voor de switch naar motor 2
    n++;
    }
 

void reference(){
   if(n%2== 0){ 
        if (button_cw == 0){
            d_ref = d_ref + w_ref * Ts;
        }
             if (d_ref > 21.36){
                d_ref = 21.36;
                //d_ref_const_cw = 1;
            }
        else{ 
            d_ref = d_ref;
        }
        
        if (button_ccw == 0){
            d_ref = d_ref - w_ref * Ts;
        }
            if (d_ref < -21.36){
            d_ref = -21.36;
            
        }
        else{
            d_ref = d_ref;
        }
  }     // haakje sluit voor if loop voor n%==0 
}

void reference2(){                    // maakt reference aan voor motor2
if(n%2 != 0){
    if (button_cw == 0){
        d_ref2 = d_ref2 + w_ref2 * Ts;
    }
         if (d_ref2 > 21.36){
            d_ref2 = 21.36;
            //d_ref_const_cw = 1;
        }
    else{ 
        d_ref2 = d_ref2;
    }
    
    if (button_ccw == 0){
        d_ref2 = d_ref2 - w_ref2 * Ts;
    }
        if (d_ref2 < -21.36){
        d_ref2 = -21.36;
        
    }
    else{
        d_ref2 = d_ref2;
    }
  } // haakje sluit voor if loop met n%!=0)  
}
    
void m1_controller(){
    error1 = d_ref-rev_counts_motor1_rad;
    ctrlOutput1 = PID_controller.step(error1);
}
void m2_controller(){                       // void voor tweede controller
    error2 = d_ref2-rev_counts_motor2_rad;
    ctrlOutput2 = PID_controller.step(error2);
    }
    
//void tickerfunctie()                // maakt een ticker functie aan
//{
//tickerflag = 1;                     // het enige wat deze functie doet is zorgen dat tickerflag 1 is
//}


int main()
{
     pc.baud(115200);  
QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);       //encoder motor 2
int counts_encoder1;                  //variabele counts aanmaken
int counts_encoder2;                  // variabele counts voor motor2
float rev_counts_motor1; 
float rev_counts_motor2;                // variabele coutns voor motor2
//t.attach(&tickerfunctie,ticker_TS);
sw3.fall(&SwitchN);
tick.attach(&reference,Ts); 
tick2.attach(&reference2,Ts);           // ticker voor reference2 attach
controllerticker.attach(&m1_controller,Ts);
controllerticker2.attach(&m2_controller,Ts); // ticker voor controller2 attach


PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
PID_controller2.PIDF(Kp2,Ki2,Kd2,N2,Ts2);

    while (true) {
       // if (button_cw == 0){
            //if (tickerflag ==1){

               // richting_motor1 = 1;
              //  pwm_motor1 = voltage_motor1;
                //pc.printf("pwm_motor is %f\r\n", pwm_motor1);
                
                counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
                counts_encoder2 = Encoder2.getPulses();
                rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); 
                rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
                rev_counts_motor2=(float)counts_encoder2/(gearboxratio*rev_rond); 
                rev_counts_motor2_rad=rev_counts_motor2*6.28318530718; 
                voltage_motor1=ctrlOutput1;
                voltage_motor2=ctrlOutput2;
            if (voltage_motor1<0){
                richting_motor1 = 0;
                }
            else   {
                richting_motor1 = 1;
                }
            if (voltage_motor2<0){
                richting_motor2 = 0;
                }
            else   {
                richting_motor2 = 1;
                }
                pwm_motor1 = fabs(voltage_motor1);
                pwm_motor2 = fabs(voltage_motor2);
                
                pc.printf("%f", pwm_motor2.read());
                pc.printf("   %f", d_ref2);
                pc.printf("   %f \r\n", rev_counts_motor2_rad);
                
    }
}