File content as of revision 10:9d7110f25908:

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

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

// dit is voor schakelaar die aan en uit wil gaan
DigitalIn toggle(PTD7);

void toggle_on()
{
}

void toggle_off()
{
    // do nothing
}

/** 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 = 0;
    float pwm_to_begin_motorB = 0;
    float pwm_to_motorB;
    float pwm_to_rechtsonder_motorA;
    float pwm_to_rechtsonder_motorB;

    const float dt = 0.002;
    float Kp = 0.001;  //0.0113
    float Ki = 0.0759;
    float Kd = 0.0004342;
    float error_t0_A = 0;
    float error_t0_B = 0;
    float error_ti_A;
    float error_ti_B;
    float error_t_1_A;
    float error_t_1_B;
    float P_regelaar_A;
    float P_regelaar_B;
    float I_regelaar_A;
    float I_regelaar_B;
    float D_regelaar_A;
    float D_regelaar_B;
    float output_regelaar_A;
    float output_regelaar_B;
    float integral_i_A;
    float integral_i_B;
    float integral_0_A = 0;
    float integral_0_B = 0;

    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

    while(1) {
        while(!toggle);
        { // wait while toggle == 0
            toggle_on();

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

            // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
            // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).

            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);

            wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

            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);

            wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

            // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.

            motordirA.write(0);
            pwm_motorA.write(.08);
            do {
                setpoint_beginA = -63;      // x-as
                pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
                wait(0.2);
                keep_in_range(&pwm_to_begin_motorA, -1, 1 );
                motordirA.write(0);
                pwm_motorA.write(pwm_to_begin_motorA);
            } while(pwm_to_begin_motorA <= 0);
            motorA.setPosition(0);
            pwm_motorA.write(0);

            wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

            // hierna moet motor A naar de rechtsonder A4. Motor A 532.

            motordirA.write(0);
            pwm_motorA.write(0.08);
            do {
                setpoint_beginA = -532;     // rechtsonder positie A4
                pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
                wait(0.2);
                keep_in_range(&pwm_to_begin_motorA, -1, 1 );
                motordirA.write(0);
                pwm_motorA.write(pwm_to_begin_motorA);
            } while(pwm_to_begin_motorA <= 0);
            pwm_motorA.write(0);

            wait(1);

            // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.

            motordirB.write(1);
            pwm_motorB.write(.08);
            do {
                setpoint_beginB = 192;      // x-as
                pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
                wait(0.2);
                keep_in_range(&pwm_to_begin_motorB, -1, 1 );
                motordirB.write(1);
                pwm_motorB.write(pwm_to_begin_motorB);
            } while(pwm_to_begin_motorB <= 0);
            motorB.setPosition(0);
            pwm_motorB.write(0);

            wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

            // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.

            motordirB.write(1);
            pwm_motorB.write(0.08);
            do {
                setpoint_beginB = 460;      // rechtsonder positie A4
                pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
                wait(0.2);
                keep_in_range(&pwm_to_begin_motorB, -1, 1 );
                motordirB.write(1);
                pwm_motorB.write(pwm_to_begin_motorB);
            } while(pwm_to_begin_motorB <= 0);
            pwm_motorB.write(0);

            wait(1);

            // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
            // Hierna het script dat EMG wordt omgezet in een positie verandering

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

            //INFINITE LOOP
            while(1) {

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

                // hier EMG
                //setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden             dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027
                //setpointB = (potmeterB.read())*(415);           // bereik van 46.7 graden
                //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
                //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
                setpointA = (potmeterA.read() - 0.5)*(631/2);
                setpointB = (potmeterB.read() - 0.5) * (871/2);
                // 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, -1105, -474);     // voor motor moet bereik zijn -1105 tot -474
                keep_in_range(&setpointB, -147, 269);       // voor motor moet bereik zijn -147 tot 269

                // PID regelaar voor motor A
                //wait(dt);
                //error_ti_A = setpointA - motorA.getPosition();
                //P_regelaar_A = Kp * error_ti_A;
                //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
                //integral_i_A = integral_0_A + (error_ti_A * dt);
                //I_regelaar_A = Ki * integral_i_A;
                //integral_0_A = integral_i_A;
                //error_t0_A = error_ti_A;
                //output_regelaar_A = P_regelaar_A;

                // PID regelaar voor motor B
                //wait(dt);
                //error_ti_B = setpointB - motorB.getPosition();
                //P_regelaar_B = Kp * error_ti_B;
                //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
                //integral_i_B = integral_0_B + (error_ti_B * dt);
                //I_regelaar_B = Ki * integral_i_B;
                //integral_0_B = integral_i_B;
                //error_t0_B = error_ti_B;
                //output_regelaar_B = P_regelaar_B;

                /* This is a PID-action! store in pwm_to_motor */
                pwm_to_motorA = (setpointA - motorA.getPosition())*.001;        //output_regelaar_A;
                pwm_to_motorB = (setpointB - motorB.getPosition())*.001;        //output_regelaar_B;

                keep_in_range(&pwm_to_motorA, -1,1);
                keep_in_range(&pwm_to_motorB, -1,1);

                if(pwm_to_motorA > 0)
                    motordirA.write(1);
                else
                    motordirA.write(0);
                if(pwm_to_motorB > 0)
                    motordirB.write(1);
                else
                    motordirB.write(0);

                pwm_motorA.write(abs(pwm_to_motorA));
                pwm_motorB.write(abs(pwm_to_motorB));
            }
        }
        while(toggle);
        {  // wait while toggle == 1
            toggle_off();
            pwm_motorA.write(0);
            pwm_motorB.write(0);
        }
    }
}


void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}