File content as of revision 0:0cd34994e6d6:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"

#define SAMPLETIME_REGELAAR         0.005
#define KP_arm2                     0.05        //Factor proprotionele regelaar arm 2
#define KI_arm2                     0           //Factor integraal regelaar arm 2
#define KD_arm2                     0           //Factor afgeleide regelaar arm 2
        #define A_TTTT 10049
        float t = 0;

PwmOut          pwm_motor_arm2(PTC8);           //PWM naar motor arm 2
DigitalOut      dir_motor_arm2(PTC9);           //Ricting van motor arm 2
Encoder         puls_motor_arm2(PTD5, PTA13);   //Encoder pulsen tellen van motor arm 2, (geel, wit)

Ticker ticker_motor_arm2_pid;
Ticker ticker_regelaar;
MODSERIAL pc(USBTX, USBRX);

float pwm_to_motor_arm2;                //PWM output naar motor arm 2
float error_arm2_kalibratie;            //Error in pulsen arm 2
float vorige_error_arm2 = 0;            //Derivative actie van regelaar arm 2
float integraal_arm2 = 0;               //Integraal actie van regelaar arm 2
float afgeleide_arm2;                   //Afgeleide actie van regelaar arm 2
float referentie_arm2 = 0;


volatile bool regelaar_ticker_flag;     //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_ticker_flag()          //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
{
    regelaar_ticker_flag = true;
}

void keep_in_range(float * in, float min, float max)        //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
{
    if (*in < min) {                    //Als de waarde kleiner is als het minimum wordt de waarde het minimum
        *in = min;
    }
    if (*in > max) {                    //Als de waarde groter is dan het maximum is de waarde het maximum
        *in = max;
    } else {                            //In alle andere gevallen is de waarde de waarde zelf
        *in = *in;
    }
}

void motor_arm2_pid()
{
    error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition());                //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
    integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR;                        //Integraal deel van regelaar
    afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR;                   //Afgeleide deel van regelaar
    pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
    vorige_error_arm2 = error_arm2_kalibratie;
    keep_in_range(&pwm_to_motor_arm2, -1, 1);                                                           //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)

    if (pwm_to_motor_arm2 > 0) {                        //Als PWM is positief, dan richting 1
        dir_motor_arm2.write(1);
    } else {                                            //Anders richting nul
        dir_motor_arm2.write(0);
    }
    pwm_motor_arm2.write(fabs(pwm_to_motor_arm2));      //Output PWM naar motor is de absolute waarde van de berekende PWM
}

int main()
{
    pc.printf("hoi");
    ticker_motor_arm2_pid.attach(motor_arm2_pid, SAMPLETIME_REGELAAR);
    ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
    
    pc.baud(38400);

    while(1) {
        
        
        while(regelaar_ticker_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
        regelaar_ticker_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
                
        referentie_arm2 = 0.5 * 10049 * t * t;
        t = t + 0.0165;

        pc.printf("referentie arm 2 %f ", referentie_arm2);
        pc.printf("get position %d ", puls_motor_arm2.getPosition());
        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
        pc.printf("t is %f\n\r", t);

        if(referentie_arm2 >= 1902) {
            while(pwm_to_motor_arm2 != 0) {
                referentie_arm2 = referentie_arm2 - 90/200;
                pc.printf("referentie arm 2 %f ", referentie_arm2);
                pc.printf("get position %d ", puls_motor_arm2.getPosition());
                pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
            }
            if(pwm_to_motor_arm2 == 0) {
                pc.printf("staat stil");
            }
        }
    }
}