File content as of revision 1:faa90344cdd8:

#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.025;                              // 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 Fs = 0.001;
const double Kp = 0.5;
const double Ki = 5;
const double Kd = 5; 
const double N = 100;
Ticker tick;
Ticker controllerticker;
float rev_counts_motor1_rad;
volatile float voltage_motor1=0.18;
volatile double error1;
volatile double ctrlOutput;
volatile double sintel =0;

void reference(){
d_ref = d_ref + w_ref * Fs;
    }
    
void m1_controller(){
    error1 = d_ref-rev_counts_motor1_rad;
    ctrlOutput = PID_controller.step(error1);
    }
void sin_teller(){
    sintel=sintel+0.01;
    }
// if (ctrlOutput > 1){
//        ctrlOutput =1;
 //       }
 //   if (ctrlOutput < 0.01){
 //       ctrlOutput = 0.01;
 //       }
 //   else{
 //       ctrlOutput = ctrlOutput;
 //       }
    



int main()
{
     pc.baud(115200);  
QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
float counts_encoder1;                  //variabele counts aanmaken
float rev_counts_motor1; 

    
    while (true) {
        if (button_cw == 0){
 t.attach(&sin_teller,1);
richting_motor1 = 1;
pwm_motor1 = sin(sintel*10*3.141592653);
pc.printf("pwm_motor is %f\r\n", pwm_motor1);
pc.printf("sintel is %f\r\n", sintel);
counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); 
rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
//tick.attach(&reference,Fs);  
//PID_controller.PIDF(Kp,Ki,Kd,N,Fs);
//controllerticker.attach(&m1_controller,Fs);
//voltage_motor1=ctrlOutput;
//pc.printf("%f", voltage_motor1);
//pc.printf("   %f", d_ref);
//pc.printf("   %f \r\n", rev_counts_motor1_rad);
Scope.set(0, rev_counts_motor1_rad);
Scope.send();
}

else{
    pwm_motor1=0;
    
}
    }
}