File content as of revision 2:83dd9068b6c5:

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

/*******************************************************************************
*                                                                              *
*   Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/  *
*                                                                              *
********************************************************************************/

/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
void keep_in_range(float * in, float min, float max);

/** variable to show when a new loop can be started*/
/** volatile means that it can be changed in an    */
/** interrupt routine, and that that change is vis-*/
/** ible in the main loop. */

volatile bool looptimerflag;

/** function called by Ticker "looptimer"     */
/** variable 'looptimerflag' is set to 'true' */
/** each time the looptimer expires.          */
void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main()
{
    //LOCAL VARIABLES
    /*Potmeter input*/
    AnalogIn potmeterA(PTC2);
    AnalogIn potmeterB(PTB2);
    /* Encoder, using my encoder library */
    /* First pin should be PTDx or PTAx  */
    /* because those pins can be used as */
    /* InterruptIn                       */
    Encoder motorA(PTD4,PTC8);
    Encoder motorB(PTD0,PTD2);
    /* MODSERIAL to get non-blocking Serial*/
    MODSERIAL pc(USBTX,USBRX);
    /* PWM control to motor */
    PwmOut pwm_motorA(PTA12);
    PwmOut pwm_motorB(PTA5);
    /* Direction pin */
    DigitalOut motordirA(PTD3);
    DigitalOut motordirB(PTD1);
    /* variable to store setpoint in */
    float setpointA;
    float setpointB;
    float setpoint_beginA;
    float setpoint_beginB;
    float setpoint_rechtsonderA;
    float setpoint_rechtsonderB;
    /* variable to store pwm value in*/
    float pwm_to_motorA;
    float pwm_to_begin_motorA;
    float pwm_to_begin_motorB;
    float pwm_to_motorB;
    float pwm_to_rechtsonder_motorA;
    float pwm_to_rechtsonder_motorB;
    int32_t positionmotorA_t0;
    int32_t positionmotorB_t0;
    int32_t positionmotorA_t_1;
    int32_t positionmotorB_t_1;
    int32_t positiondifference_motorA;
    int32_t positiondifference_motorB;

    //START OF CODE

    /*Set the baudrate (use this number in RealTerm too! */
    pc.baud(921600);


    // in dit stukje code zorgen we ervoor dat de startpositie wordt bereikt. Eerst B botsen dan A botsen
    motordirB.write(0);
    pwm_motorB.write(.08);
    positionmotorB_t0 = motorB.getPosition();
    do {
        wait(0.2);
        positionmotorB_t_1 = positionmotorB_t0;
        positionmotorB_t0 = motorB.getPosition();
        positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
    } while(positiondifference_motorB > 10);
    motorB.setPosition(0);
    pwm_motorB.write(0);

    motordirA.write(1);
    pwm_motorA.write(.08);
    positionmotorA_t0 = motorA.getPosition();
    do {
        wait(0.2);
        positionmotorA_t_1 = positionmotorA_t0 ;
        positionmotorA_t0 = motorA.getPosition();
        positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
    } while(positiondifference_motorA > 10);
    motorA.setPosition(0);
    pwm_motorA.write(0);

    // motor A moet als eerst naar x-as, moet 4.11 graden naar links om op de x-as te staan
    // motor B moet 21.6 graden naar links.
    // eerst motor A en dan motor B

    // motordirA.write(1);
    //setpoint_beginA = 63.5; // naar x-as gaan
    //pwm_to_begin_motorA = setpoint_beginA *.001;
    //keep_in_range(&pwm_to_begin_motorA, -1,1);
    //pwm_motorA.write(abs(pwm_to_begin_motorA));
    //motorA.setPosition(0);
    //pwm_motorA.write(0);

    //motordirB.write(1);
    //setpoint_beginB = 192; // naar x-as gaan
    //pwm_to_begin_motorB = setpoint_beginB *.001;
    //keep_in_range(&pwm_to_begin_motorB, -1,1);
    //pwm_motorB.write(abs(pwm_to_begin_motorB));
    //motorB.setPosition(0);
    //pwm_motorB.write(0);
    Ticker looptimer1;
    looptimer1.attach(setlooptimerflag,0.01);

    while(pwm_to_begin_motorA >= 0) {
        while(looptimerflag != true);

        looptimerflag = false;

        setpoint_beginA = 63;     // naar x-as gaan
        pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001;
        keep_in_range(&pwm_to_begin_motorA, -1, 1 );

        //if(pwm_to_begin_motorA <= 0) {
        //  motordirA.write(0);
        // motorA.setPosition(0);
        //pwm_motorA.write(0);
        //} else {
        motordirA.write(0);
        pwm_motorA.write(abs(pwm_to_begin_motorA));
    }
motorA.setPosition(0);
pwm_motorA.write(0);

Ticker looptimer2;
looptimer2.attach(setlooptimerflag,0.01);

while(pwm_to_begin_motorB >= 0)
{
    while(looptimerflag != true);
    looptimerflag = false;

    setpoint_beginB = 192; //192
    pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
    keep_in_range(&pwm_to_begin_motorB, -1,1);
    //if(pwm_to_begin_motorB <= 0) {
    //  motordirB.write(0);
    //motorB.setPosition(0);
    //pwm_motorB.write(0);
    //} else {
    motordirB.write(1);
    pwm_motorB.write(abs(pwm_to_begin_motorB));
}
motorB.setPosition(0);
pwm_motorB.write(0);

// nu naar positie rechtsonderhoek A4 deze is motor A 531.6 en motor B 460
// verder werken vanaf hier
while(0) ///1
{
    while(looptimerflag != true);

    looptimerflag = false;

    setpoint_rechtsonderA = 531;     // naar rechter onderhoek
    pwm_to_rechtsonder_motorA = setpoint_rechtsonderA *.001;
    keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
    if(pwm_to_rechtsonder_motorA > 0.531) {
        motordirA.write(0);
        pwm_motorA.write(0);
    } else
        motordirA.write(1);
    pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));

    while(looptimerflag != true);
    looptimerflag = false;

    setpoint_rechtsonderB = 460;
    pwm_to_rechtsonder_motorB = setpoint_rechtsonderB *.001;
    keep_in_range(&pwm_to_rechtsonder_motorB, -1,1);
    if(pwm_to_rechtsonder_motorB > 0.460) {
        motordirB.write(0);
        pwm_motorB.write(0);
    } else
        motordirB.write(1);
    pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));

}


/*Create a ticker, and let it call the     */
/*function 'setlooptimerflag' every 0.01s  */
Ticker looptimer;
looptimer.attach(setlooptimerflag,0.01);

//INFINITE LOOP
while(1)
{
    /* Wait until looptimer flag is true. */
    /* '!=' means not-is-equal            */
    while(looptimerflag != true);
    /* Clear the looptimerflag, otherwise */
    /* the program would simply continue  */
    /* without waitingin the next iteration*/
    looptimerflag = false;




    /* Read potmeter value, apply some math */
    /* to get useful setpoint value         */
    setpointA = (potmeterA.read()-0.5)*(631/2); // bereik van 71 graden             dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen
    setpointB = (potmeterB.read()-0.5)*(415/2);  // bereik van 46.7 graden           1000 dan rotatie langzamer maken als lager maakt.


    // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
    // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
    keep_in_range(&setpointA, 474, 1105);
    keep_in_range(&setpointB, -147, 269);

    /* Print setpoint and current position to serial terminal*/
    pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
    pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());

    /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
    pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
    pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
    /* Coerce pwm value if outside range          */
    /* Not strictly needed here, but useful       */
    /* if doing other calculations with pwm value */
    keep_in_range(&pwm_to_motorA, -1,1);
    keep_in_range(&pwm_to_motorB, -1,1);

    /* Control the motor direction pin. based on   */
    /* the sign of your pwm value.      If your    */
    /* motor keeps spinning when running this code */
    /* you probably need to swap the motor wires,  */
    /* or swap the 'write(1)' and 'write(0)' below */
    if(pwm_to_motorA > 0)
        motordirA.write(1);
    else
        motordirA.write(0);
    if(pwm_to_motorB > 0)
        motordirB.write(1);
    else
        motordirB.write(0);


    //WRITE VALUE TO MOTOR
    /* Take the absolute value of the PWM to send */
    /* to the motor. */
    pwm_motorA.write(abs(pwm_to_motorA));
    pwm_motorB.write(abs(pwm_to_motorB));
}
}


//coerces value 'in' to min or max when exceeding those values
//if you'd like to understand the statement below take a google for
//'ternary operators'.
void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}