File content as of revision 0:16ee9fc421f8:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "PwmOut.h"
/*definieren pinnen Motor 1*/
#define M1_PWM PTA5
#define M1_DIR PTA4
#define M2_PWM PTC8
#define M2_DIR PTC9
/*Definieren minimale waarden PWM per motor*/
#define PWM1_min_50 0.529 /*met koppelstuk!*/
#define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
/*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
#define TSAMP 0.005
#define K_P (100)
#define K_I (50 * TSAMP)
#define K_D (1)
//#define K_D (0.0005 /TSAMP)
#define I_LIMIT 100.
#define PI 3.1415926535897
#define lengte_arm 0.5
 
Encoder motor1(PTD3,PTD1, true);
PwmOut pwm_motor1(M1_PWM);
DigitalOut motordir1(M1_DIR);
void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
volatile bool looptimerflag;
float PWM1_percentage = 0;
int PWM2_percentage = 100; 
int aantal_rotaties_1 = 10;
int aantalcr_1 = 1600;
int aantalcr_2 = 960; 
int beginpos_motor1;
int beginpos_motor1_new; 
int beginpos_motor2;
int beginpos_motor2_new;
uint16_t gewenste_snelheid = 2;
uint16_t gewenste_snelheid_rad = 4; 
int previous_pos_motor1 = 0;
int current_pos_motor1;
float pos_motor1_rad;
int delta_pos_motor1_puls;
float delta_pos_motor1_rad;
float snelheid_motor1_rad;
float snelheid_arm_ms; 
float PWM1; 
float Speed_motor1;
float Speed_motor1rad;
 
void setlooptimerflag(void) //Ticker maakt een constructie die zoveel keer per seconde een functie uitvoerd,  met sampingling freq TSAMP
{
    looptimerflag = true;
}
    
int main()
{
    motor1.setPosition(0);
    pwm_motor1.period_us(100);
    float setpoint;
    float prev_setpoint = 0; 
    Ticker looptimer; 
    looptimer.attach(setlooptimerflag,TSAMP);   
    
    while(true)
    {
            while(!looptimerflag) //Hierdoor zeg je eigenlijk tegen je systeem; voer dit script uit na elke TSAMP s. 
            {
            /*do nothing*/  //Zolang dit waar is, doe je niets. 
            }
        looptimerflag = false; //clear flag
                
        current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
        pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
        
        //begrenzen van de hoek(in dit geval 90 graden)
        if (current_pos_motor1 >= 400)
        gewenste_snelheid_rad = 0; 
        else
        
        setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;       
        PWM1_percentage = pid(setpoint, pos_motor1_rad);
       
// zorgen dat PWM1_percentage niet buiten de range van 100% valt       
        if (PWM1_percentage < -100)
            PWM1_percentage = -100;
        else if (PWM1_percentage >100)
            PWM1_percentage =100;
        else
        
        // bepalen van de richting
        if(PWM1_percentage < 0)
            motordir1 = 1;
        else
            motordir1 = 0;
            // ennnn uitsturen van PWM 
        pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
        scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
        prev_setpoint = setpoint;       
    }    
}

void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op
// maar de locatie van de variabele. 
{
    *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
    // *in = het getal dat staat op locatie van in --> waarde van new_pwm
}


float pid(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = (setpoint-measurement);
    out_p  = error*K_P; 
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    return out_p + out_i + out_d;
}