Revision 0:6ca6892a17d0, committed 2013-11-04

Comitter:

Tess

Date:

Mon Nov 04 23:09:57 2013 +0000

Commit message:

Aansturing motor met POT, niet werkend

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Mon Nov 04 23:09:57 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/vsluiter/code/Encoder/#0998a8fd7727

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Mon Nov 04 23:09:57 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#f42def64c4ee

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 04 23:09:57 2013 +0000
@@ -0,0 +1,317 @@
+#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 visible 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
+    AnalogIn potmeterA(PTC2);
+    AnalogIn potmeterB(PTB2);
+    Encoder motorA(PTD4,PTC8);
+    Encoder motorB(PTD0,PTD2);
+    MODSERIAL pc(USBTX,USBRX);      // MODSERIAL to get non-blocking Serial
+    PwmOut pwm_motorA(PTA12);       // PWM control to motor
+    PwmOut pwm_motorB(PTA5);        // PWM control to motor
+    DigitalOut motordirA(PTD3);     // Direction pin
+    DigitalOut motordirB(PTD1);     // Direction pin
+
+    /* 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;
+
+    /* variable for PD controller*/
+    const float dt = 0.002;
+    float Kp = 0.001;               //0.0208
+    float Kd = 0.00004342;          //0.0006897
+    float error_t0_A = 0;
+    float error_t0_B = 0;
+    float error_ti_A;
+    float error_ti_B;
+    float P_regelaar_A;
+    float P_regelaar_B;
+    float D_regelaar_A;
+    float D_regelaar_B;
+    float output_regelaar_A;
+    float output_regelaar_B;
+
+    /* variable to store positions in*/
+    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;
+
+    /* inverse kinematica */
+    float dy;       //dy waarde tussen -1 en 1     -1 -vmax; 1 vmax
+    float dx;       //dx waarde tussen -1 en 1     -1 -vmax; 1 vmax
+    const float vmax = 0.08;          // m/s
+    const float delta_t = 0.005;  // 1/samplefrequentie, dus tijd tussen twee meetpunten
+    float X_positie;
+    float Y_positie;
+    float X_positie_begin;
+    float Y_positie_begin;
+    float puls_motorA;
+    float puls_motorB;
+    float kwadraat_X_positie;
+    float kwadraat_Y_positie;
+    float phi_A_pulsen_positie_begin;
+    float phi_B_pulsen_positie_begin;
+    float phi_A_positie_begin;
+    float phi_B_positie_begin;
+    float phi_1;
+    float lengte_arm = 276;     // in mm anders rare imaginaire getallen
+    float phi_A;
+    float phi_B;
+    float Puls_motorA;
+    float Puls_motorB;
+    float phi_A_pulsen;
+    float phi_B_pulsen;
+    float pi = 3.14159265;
+
+    //START OF CODE
+
+    pc.baud(115200);                // Set the baudrate (use this number in RealTerm too!)
+
+    // 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(.1);   //0.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(.1);
+    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 {
+        wait(0.01);
+        setpoint_beginA = -63;      // x-as
+        pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001;   // + omdat men met een negatieve hoekverdraaiing werkt.
+        keep_in_range(&pwm_to_begin_motorA, -0.3, 0.3 );
+        motordirA.write(0);
+        pwm_motorA.write(abs(pwm_to_begin_motorA));
+        pc.printf("s: %d, %f \n\r", motorA.getPosition(), 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 (hoek 59.8 graden).
+
+    motordirA.write(0);
+    pwm_motorA.write(0.08);
+    do {
+        wait(0.01);
+        setpoint_rechtsonderA = -532;     // rechtsonder positie A4
+        pwm_to_rechtsonder_motorA = (setpoint_rechtsonderA - motorA.getPosition()) *.001;
+        keep_in_range(&pwm_to_rechtsonder_motorA, -0.3, 0.3 );
+        motordirA.write(0);
+        pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));
+    } while(pwm_to_rechtsonder_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 {
+        wait(0.01);
+        setpoint_beginB = 192;      // x-as
+        pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
+        keep_in_range(&pwm_to_begin_motorB, -0.3, 0.3 );
+        motordirB.write(1);
+        pwm_motorB.write(abs(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 268 (30.2 graden).
+
+    motordirB.write(1);
+    pwm_motorB.write(0.08);
+    do {
+        wait(0.01);
+        setpoint_rechtsonderB = 268;      // rechtsonder positie A4
+        pwm_to_rechtsonder_motorB = (setpoint_rechtsonderB - motorB.getPosition()) *.001;
+        keep_in_range(&pwm_to_rechtsonder_motorB, -0.3, 0.3 );
+        motordirB.write(1);
+        pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
+    } while(pwm_to_rechtsonder_motorB >= 0);
+    pwm_motorB.write(0);
+
+    wait(1);
+
+    // Nu zijn de motoren gekalibreed en staan ze op de startpositie (rechtsonderhoek van A4).
+    // 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;
+
+        dx = ((potmeterA.read()-0.5)*2000);
+        dy = ((potmeterB.read()-0.5)*2000);  //waarde uit tussen -1 en 1
+
+        // inverse kinematica
+        phi_A_pulsen_positie_begin = motorA.getPosition();
+        phi_B_pulsen_positie_begin = motorB.getPosition();
+        //pc.printf("s: %f, %d \n\r", phi_A_pulsen_positie_begin, phi_B_pulsen_positie_begin);
+
+        phi_A_positie_begin = (360/3200.0) * phi_A_pulsen_positie_begin * (pi/180);
+        phi_B_positie_begin = (360/3200.0) * phi_B_pulsen_positie_begin * (pi/180);
+
+        phi_1 = phi_A_positie_begin - phi_B_positie_begin;
+
+        X_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * cos(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
+        Y_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * sin(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
+
+        X_positie = dx * vmax * delta_t + X_positie_begin;          // delta_t eruit want dx tussen -1 en 1 en dimensieloos
+        Y_positie = dy * vmax * delta_t + Y_positie_begin;
+
+        kwadraat_X_positie = pow(X_positie,2);
+        kwadraat_Y_positie = pow(Y_positie,2);
+
+        phi_A = pi - acos(sqrt(kwadraat_X_positie+kwadraat_Y_positie)/(2*lengte_arm)) - atan(Y_positie/X_positie);      //rad
+        phi_B = pi - phi_A - acos(-(kwadraat_X_positie + kwadraat_Y_positie) / (2 * pow(lengte_arm,2))+1);
+
+        phi_A_pulsen = (3200/(2*pi)) * phi_A;
+        phi_B_pulsen = (3200/(2*pi)) * phi_B;
+
+        // 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(&phi_A_pulsen, -1104, -474);     // voor motor moet bereik zijn -1104 tot -474
+        keep_in_range(&phi_B_pulsen, -146, 268);       // voor motor moet bereik zijn -146 tot 268
+
+        Puls_motorA = phi_A_pulsen - phi_A_pulsen_positie_begin;
+        Puls_motorB = phi_B_pulsen - phi_B_pulsen_positie_begin;
+
+        // doe begin berekening
+        pwm_to_motorA = Puls_motorA*.001;
+        pwm_to_motorB = Puls_motorB*.001;
+
+        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));
+
+
+        /*/PD regelaar voor motor A
+        wait(dt);
+        error_ti_A = phi_A_pulsen - motorA.getPosition();                   // puls_motorA - motorA.getPosition();
+        P_regelaar_A = Kp * error_ti_A;
+        D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
+        error_t0_A = error_ti_A;
+        output_regelaar_A = P_regelaar_A + D_regelaar_A;
+
+        //PD regelaar voor motor B
+        wait(dt);
+        error_ti_B = phi_B_pulsen - motorB.getPosition();                           //puls_motorB - motorB.getPosition();
+        P_regelaar_B = Kp * error_ti_B;
+        D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
+        error_t0_B = error_ti_B;
+        output_regelaar_B = P_regelaar_B + D_regelaar_B;
+        `*/
+        /* This is a PD-action! store in pwm_to_motor */
+        /*pwm_to_motorA = output_regelaar_A;
+        pwm_to_motorB = 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));*/
+    }
+}
+
+void keep_in_range(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 04 23:09:57 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file