File content as of revision 25:f45d61cc6dc6:

#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);
//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;
int staat = 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 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)
    cur_pos_motor1 = motor1.getPosition();
    pos_motor1_rad = (float)cur_pos_motor1/(3600.0/(2.0*PI));
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (180*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (180*2.0*PI/360);
    }
    if(setpoint1 < -(180*2.0*PI/360)) {
        setpoint1 = -(180*2.0*PI/360);
    }

    pwm_motor1.write(abs(pwm1_percentage));
    prev_setpoint1 = setpoint1;
    wait(TSAMP);

}

void batje_rechts ()
{
    speed1_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
    cur_pos_motor1 = motor1.getPosition();
    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (360*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (360*2.0*PI/360);
    }
    if(setpoint1 < -(360*2.0*PI/360)) {
        setpoint1 = -(360*2.0*PI/360);
    }
    pwm_motor1.write(abs(pwm1_percentage));
    prev_setpoint1 = setpoint1;
    if(setpoint1 > (360*2.0*PI/360)) {
        staat = 1;
    }
    wait(TSAMP);


}

void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
{
    speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
    cur_pos_motor1 = motor1.getPosition();
    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (155.0*2.0*PI/360);
    }
    if(setpoint1 < -(155.0*2.0*PI/360)) {
        setpoint1 = -(155.0*2.0*PI/360);
    }
    pwm_motor1.write(abs(pwm1_percentage));
    prev_setpoint1 = setpoint1;
   //if(setpoint1 = (155.0*2.0*PI/360)) {
     // staat = 1;
    //}
    wait(TSAMP);

}

void arm_mid ()
{
    speed1_rad = -3.0; //positief is CCW, negatief CW (boven aanzicht)
    cur_pos_motor1 = motor1.getPosition();
    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
    if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
        setpoint1 = (155*2.0*PI/360);
    }
    if(setpoint1 < -(155*2.0*PI/360)) {
        setpoint1 = -(155*2.0*PI/360);
    }
    pwm_motor1.write(abs(pwm1_percentage));
    prev_setpoint1 = setpoint1;
    
    wait(TSAMP);
}

void arm_laag ()
{
    speed2_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
    cur_pos_motor2 = motor2.getPosition();
    pos_motor2_rad = (float)cur_pos_motor2/(3600/(2.0*PI));
    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
    if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
        setpoint2 = (100*2.0*PI/360);
    }
    if(setpoint2 < -(100*2.0*PI/360)) {
        setpoint2 = -(100*2.0*PI/360);
    }

    pwm_motor2.write(abs(pwm2_percentage));
    prev_setpoint2 = setpoint2;
    wait(TSAMP);
}

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

    while(1) {
        pc.printf("%d \r\n", motor1.getPosition());
        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;
        }

        if(pwm1_percentage > 0) {
            motordir1 = 0;
        } else {
            motordir1 = 1;
        }


        if(staat == 0) {
            arm_hoog();
        } 
        else 
        {
            arm_mid();
        }
       }
}