File content as of revision 24:7fbd904d191c:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "PwmOut.h"

#define TSAMP 0.005
#define K_P (3.5)
#define K_I (0.01  *TSAMP)
#define I_LIMIT 1.
#define PI 3.14159265
#define l_arm 0.5

#define M1_PWM PTC8
#define M1_DIR PTC9
#define M2_PWM PTA5
#define M2_DIR PTA4

//Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting

Serial pc(USBTX, USBRX);
DigitalOut led1(LED_RED);
HIDScope scope(2);

//motor 25D
Encoder motor1(PTD3,PTD5); //wit, geel
PwmOut pwm_motor1(M2_PWM);
DigitalOut motordir1(M2_DIR);

//motor2 37D
Encoder motor2(PTD2, PTD0); //wit, geel
PwmOut pwm_motor2(M1_PWM);
DigitalOut motordir2(M1_DIR);
void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
float pwm1_percentage = 0;
float pwm2_percentage = 0;
int cur_pos_motor1;
int prev_pos_motor1 = 0;
int cur_pos_motor2;
int prev_pos_motor2 = 0;
float speed1_rad;
float speed2_rad;
float pos_motor1_rad;
float pos_motor2_rad;





void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele 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 cur_pos_motor1)
{
    float error;
    float           out_p = 0;
    static float    out_i = 0;
    error  = (setpoint - cur_pos_motor1);
    out_p  = error*K_P;
    out_i += error*K_I;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    return out_p + out_i;
}

/*void batje_links ()
{
    speed1_rad = -1.5; //positief is CCW, negatief CW (boven aanzicht)
        cur_pos_motor1 = motor1.getPosition();
        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
        setpoint = prev_setpoint + TSAMP * speed1_rad;
        if(setpoint > (11.3*2.0*PI/360)) { //setpoint in graden
            setpoint = (11.3*2.0*PI/360);
        }
        if(setpoint < -(11.3*2.0*PI/360)) {
            setpoint = -(11.3*2.0*PI/360);
        }
        
        pwm_motor1.write(abs(pwm1_percentage));
        prev_setpoint = setpoint;
        wait(TSAMP);
}

void batje_rechts ()
{
    speed1_rad = 1.5; //positief is CCW, negatief CW (boven aanzicht)
        cur_pos_motor1 = motor1.getPosition();
        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
        setpoint = prev_setpoint + TSAMP * speed1_rad;
        if(setpoint > (11.3*2.0*PI/360)) { //setpoint in graden
            setpoint = (11.3*2.0*PI/360);
        }
        if(setpoint < -(11.3*2.0*PI/360)) {
            setpoint = -(11.3*2.0*PI/360);
        }
        pwm_motor1.write(abs(pwm1_percentage));
        prev_setpoint = setpoint;
}*/
int main()
{
        pwm_motor1.period_us(100);
        motor1.setPosition(0);
        float prev_setpoint = 0;
        float setpoint = 0;
        
    while(1) {

        pwm1_percentage = pid(setpoint, pos_motor1_rad);
        if (pwm1_percentage < -1.0) {
            pwm1_percentage = -1.0;
        }
        if (pwm1_percentage > 1.0) {
            pwm1_percentage = 1.0;
        }

        if(pwm1_percentage > 0) {
            motordir1 = 0;
        } else {
            motordir1 = 1;
        }
        
        speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
        cur_pos_motor1 = motor1.getPosition();
        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
        setpoint = prev_setpoint + TSAMP * speed1_rad;
        if(setpoint > (12*2.0*PI/360)) { //setpoint in graden
            setpoint = (12*2.0*PI/360);
        }
        if(setpoint < -(12*2.0*PI/360)) {
            setpoint = -(12*2.0*PI/360);
        }
        
        pwm_motor1.write(abs(pwm1_percentage));
        prev_setpoint = setpoint;
        wait(TSAMP);
    }
}