File content as of revision 3:0de66e6f1a80:

#include "mbed.h"
#include "Servo.h"
#include <math.h>

#define PI 3.14159265


//------------------------------------
// Hyperterminal configuration
// 9600 bauds, 8-bit data, no parity
//------------------------------------

Serial pc(SERIAL_TX, SERIAL_RX);

// configuration
Servo myservo0(D3);                // Create the servo object
Servo myservo1(D5);                // Create the servo object
Servo myservo2(D6);                // Create the servo object
Servo myservo3(D9);                // Create the servo object
AnalogIn knob0(A0);                // Create the analog input object
AnalogIn knob1(A1);                // Create the analog input object
AnalogIn knob2(A2);                // Create the analog input object
AnalogIn knob3(A3);                // Create the analog input object

// presser le bouton B1 pour
// lancer le parcours de test()
DigitalIn monBouton(PC_13);



//fonction ALLER
//pour déplacer trois servos avec ralentissement à la fin :
//servo0 :  position actuelle -> fin0
//servo1 :  position actuelle -> fin1
//servo2 :  position actuelle -> fin0
void aller(float fin0, float fin1)
{
    float debut0 = myservo0.read();
//    float fin0=0.5;
    float debut1 = myservo1.read();
//    float fin1=1;
    float debut2 = debut0;
    float fin2 = fin0;
    int i;

    for(i=0; i<100; i++) {
        // remplacement du coefficient i/100 par un sinus
        // et donc mouvement plus fluide
        myservo1.write (debut1 + sin(i*PI/2.0/100.0) * (fin1-debut1));
        wait_ms(2);
        myservo0.write (debut0 + sin(i*PI/2.0/100.0) * (fin0-debut0));
        wait_ms(2);
        myservo2.write (debut2 + sin(i*PI/2.0/100.0) * (fin2-debut2));
        wait_ms(2);
        wait_ms(10);

    }

    myservo0.write(fin0);
    wait_ms(15);
    myservo1.write(fin1);
    wait_ms(15);
    myservo2.write(fin2);
    wait_ms(15);
}



// fonction MAP
// pour faire de la translation d'intervalle
// i dans [a ; b] devient j dans [c ; d]
float map (float i, float a, float b, float c, float d)
{
    float j;
    j = (d * (i-a) - c * (i-b) ) / (b-a);
    return j;
}



// fonctino BEZIER
// pour pour passer de 3 intervalles continus
// à un seul lissé
// i paramètre dans [ borne_inf ; borne_sup]
// intervalle 1 : [debut ; intermed0]
// intervalle 2 : [intermed0 ; intermed1]
// intervalle 3 ; [intermed1 ; fin]
float bezier(int i , int borne_inf , int borne_sup , float debut , float intermed0 , float intermed1 , float fin)
{
    float pt[6];
    pt[0] = debut;
    pt[1] = debut + (intermed0 - debut)*0.05;
    pt[2] = intermed0;
    pt[3] = intermed1;
    pt[4] = fin - (fin-intermed1)*0.05;
    pt[5] = fin;
    //
    for (int j = 5 ; j>0 ; j--) {
        for (int k = 0 ; k<j ; k++) {
            pt[k] = map (i , borne_inf, borne_sup , pt[k] , pt[k+1]) ;
        }
    }
    return pt[0];
}



// fonction ALLERPAR
// pour le servo0 : intermed00 , intermed01 et fin 0
// pour le servo1 : intermed10 , intermed11 et fin 1
// un paramètre de vitesse au plus c'est grand au plus c'est lent
void allerPar(float intermed00,float intermed01,  float fin0, float intermed10,float intermed11,float fin1, float vitesse)
{
    float debut0 = myservo0.read();
    float debut1 = myservo1.read();
    float fin2 = fin0;
    int i;
    float valeur ;

    for(i=0; i<vitesse; i++) {
        //
        valeur = bezier ( i , 0 , vitesse , debut0 , intermed00 , intermed01 , fin0);
        //
        myservo0.write ( valeur );
        wait_ms(2);
        //
        myservo2.write ( valeur );
        wait_ms(2);
        //
        valeur = bezier ( i , 0 , vitesse , debut1 , intermed10 , intermed11 , fin1);
        //
        myservo1.write ( valeur );
        wait_ms(2);
        //
        wait_ms(10);

    }

    myservo0.write(fin0);
    wait_ms(15);
    myservo1.write(fin1);
    wait_ms(15);
    myservo2.write(fin2);
    wait_ms(15);
}



// fonction TEST
// petit parcours qui se réalise lorsque l'on presse monBouton (B1)
void test()
{
    // point de départ
    aller(0.15,0.6);
    wait_ms(3000);
    //
    // petite promenade pour monter en 0,5 - 1 - 0,5
    //servo 0 et servo 2    : 0.3 -> 1.2 -> 0.5
    //servo 1               : 0 -> -0.3 -> 1
    //vitesse               : 250
    //
    allerPar(0.3 ,1.2,  0.5 , 0, -0.3 , 1, 250);
    wait_ms(25);
}



int main()
{
//calibration
// pour le servo tout merdique arduino :
// myservo1.calibrate(0.00075, 70.0); // Calibrate the servo GROOVE tout petit tout mini
    myservo0.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
    myservo1.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
    myservo2.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
    myservo3.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
    wait_ms(2000);

//boucle
    while(1) {
        // teste l'appuie sur le bouton
        if (monBouton==0) {
            test();
        }
    }
}