File content as of revision 2:ca645c585a03:

#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;
//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_ref_new;
const float w_ref = 3;
const double Ts = 0.001;
const double Kp = 0.3823;
const double Ki = 0.127875;
const double Kd = 0.2519; 
const double N = 100;
Ticker tick;
Ticker controllerticker;
float rev_counts_motor1_rad;
volatile float voltage_motor1=0.5;
volatile double error1;
volatile double ctrlOutput;
volatile double d_ref_const_cw = 0;
volatile double d_ref_const_ccw = 0;


void reference(){
    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;
    }
    
}
    
void m1_controller(){
    error1 = d_ref-rev_counts_motor1_rad;
    ctrlOutput = PID_controller.step(error1);
}
    
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 Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
int counts_encoder1;                  //variabele counts aanmaken
float rev_counts_motor1; 
t.attach(&tickerfunctie,ticker_TS);
tick.attach(&reference,Ts); 
controllerticker.attach(&m1_controller,Ts);
PID_controller.PIDF(Kp,Ki,Kd,N,Ts);

    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
                rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); 
                rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
                voltage_motor1=ctrlOutput;
            if (voltage_motor1<0){
                richting_motor1 = 0;
                }
            else   {
                richting_motor1 = 1;
                }
                pwm_motor1 = fabs(voltage_motor1);
                
                pc.printf("%f", pwm_motor1.read());
                pc.printf("   %f", d_ref);
                pc.printf("   %f \r\n", rev_counts_motor1_rad);
                //Scope.set(0, rev_counts_motor1_rad);
                //Scope.send();
                //tickerflag = 0;
              //  d_ref_const_ccw = 0;
            //}
      //  }
       // else if (button_ccw == 0){
    
               /* richting_motor1 = 0;
                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
                rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); 
                rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
                voltage_motor1=ctrlOutput;
                pc.printf("%f", pwm_motor1);
                pc.printf("   %f", d_ref);
                pc.printf("   %f \r\n", rev_counts_motor1_rad);
                //Scope.set(0, rev_counts_motor1_rad);
                //Scope.send();
                //tickerflag = 0;
                d_ref_const_cw = 0; */
  //  }

     //   else{
      //      pwm_motor1=0;
    
        //}
    }
}