File content as of revision 33:0417920456d0:

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

#define TSAMP 0.005
#define K_P1 (3.5) //voor motor1 is het 3.5
#define K_I1 (0.01  *TSAMP) //voor motor1 is het 0.01
#define K_P2 (1.0)
#define K_I2 (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);

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;
int staat1 = 0;
int staat2 = 0;

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 pid1(float setpoint1, float measurement1)
{
    float error1;
    float           out_p1 = 0;
    static float    out_i1 = 0;
    error1  = (setpoint1 - measurement1);
    out_p1  = error1*K_P1;
    out_i1 += error1*K_I1;
    clamp(&out_i1,-I_LIMIT,I_LIMIT);
    return out_p1 + out_i1;
}

float pid2(float setpoint2, float measurement2)
{
    float error2;
    float           out_p2 = 0;
    static float    out_i2 = 0;
    error2  = (setpoint2 - measurement2);
    out_p2  = error2*K_P2;
    out_i2 += error2*K_I2;
    clamp(&out_i2,-I_LIMIT,I_LIMIT);
    return out_p2 + out_i2;
}
float prev_setpoint1 = 0;
float setpoint1 = 0;
float prev_setpoint2 = 0;
float setpoint2 = 0;

void batje_links ()
{
    speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (180*2.3*2.0*PI/360);
    }
    if(setpoint1 < -(180*2.3*2.0*PI/360)) {
        setpoint1 = -(180*2.3*2.0*PI/360);
    }
    prev_setpoint1 = setpoint1;
}

void batje_rechts ()
{
    speed1_rad = 3.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (11.3*2.3*2.0*PI/360);
    }
    if(setpoint1 < -(360*2.3*2.0*PI/360)) {
        setpoint1 = -(360*2.3*2.0*PI/360);
    }
    pwm_motor1.write(abs(pwm1_percentage));
    prev_setpoint1 = setpoint1;
    if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
        staat1 = 1;
    }
}


void batje_begin_links ()
{
    speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (180*2.3*2.0*PI/360);
    }
    if(setpoint1 < -(180*2.3*2.0*PI/360)) {
        setpoint1 = -(180*2.3*2.0*PI/360);
    }
    prev_setpoint1 = setpoint1;
}

void batje_begin_rechts ()
{
    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (180*2.3*2.0*PI/360);
    }
    if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
        setpoint1 = -(0.0*2.3*2.0*PI/360);
    }
    prev_setpoint1 = setpoint1;
}

void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
{
    speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
    if(setpoint2 > (360.0*2.0*PI/360)) { //setpoint in graden
        setpoint2 = (360.0*2.0*PI/360);
    }
    if(setpoint2 < -(360.0*2.0*PI/360)) {
        setpoint2 = -(360.0*2.0*PI/360);
    }
    prev_setpoint2 = setpoint2;
    if(setpoint2 >= (360.0*2.0*PI/360)-0.1) {
        staat2 = 1;
    }
}

void arm_mid ()
{
    speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
    if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
        setpoint2 = (155.0*2.0*PI/360);
    }
    if(setpoint2 < -(155.0*2.0*PI/360)) {
        setpoint2 = -(155.0*2.0*PI/360);
    }
    prev_setpoint2 = setpoint2;
    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
        staat2 = 1;
    }
}

void arm_laag ()
{
    speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
    if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
        setpoint2 = (155.0*2.0*PI/360);
    }
    if(setpoint2 < -(155.0*2.0*PI/360)) {
        setpoint2 = -(155.0*2.0*PI/360);
    }
    prev_setpoint2 = setpoint2;
    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
        staat2 = 1;
    }
}

void arm_begin ()
{
    speed2_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
    if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
        setpoint2 = (0.0*2.0*PI/360);
    }
    if(setpoint2 < -(0.0*2.0*PI/360)) {
        setpoint2 = -(0.0*2.0*PI/360);
    }
    prev_setpoint2 = setpoint2;
}

int main()
{
    int wait_iterator1 = 0;
    int wait_iterator2 = 0;
    pwm_motor1.period_us(100);
    motor1.setPosition(0);
    pwm_motor2.period_us(100);
    motor2.setPosition(0);
    pc.baud(115200);

    while(1) {
        //MOTOR1
        pc.printf("%d \r\n", motor1.getPosition());
        cur_pos_motor1 = motor1.getPosition();
        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
        pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
        if (pwm1_percentage < -1.0) {
            pwm1_percentage = -1.0;
        }
        if (pwm1_percentage > 1.0) {
            pwm1_percentage = 1.0;
        }
        pwm_motor1.write(abs(pwm1_percentage));
        if(pwm1_percentage > 0) {
            motordir1 = 0;
        } else {
            motordir1 = 1;
        }

        //MOTOR2
        cur_pos_motor2 = motor2.getPosition();
        pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
        pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
        if (pwm2_percentage < -1.0) {
            pwm2_percentage = -1.0;
        }
        if (pwm2_percentage > 1.0) {
            pwm2_percentage = 1.0;
        }
        pwm_motor2.write(abs(pwm2_percentage));
        if(pwm2_percentage > 0) {
            motordir2 = 0;
        } else {
            motordir2 = 1;
        }
        
        wait(TSAMP);
       
        //STATES
        if(staat1 == 0) {
            batje_rechts();
            wait_iterator1 = 0;
        } else if(staat1 ==1) {
            wait_iterator1++;
            if(wait_iterator1 > 600)
                staat1 = 2;
        } else {
            batje_begin_rechts();
        }


        if(staat2 == 0) {
            arm_hoog();
            wait_iterator2 = 0;
        } else if(staat2 == 1) {
            wait_iterator2++;
            if(wait_iterator2 > 200)
                staat2 = 2;
        } else {
            arm_begin();
        }
    }
}