File content as of revision 3:1dba6eca01ad:

#define _POSIX_C_SOURCE 199309L
#include "mbed.h"
#include "reglages.h"
#include "hardware.h"
#include "DevSPI.h"
#include "XNucleoIHM02A1.h"
#include "commande_moteurs.h"

// PWM_MAX est définit dans réglage;
bool moteurs_arret = false;

//mot G
XNucleoIHM02A1 *x_nucleo_ihm02a1; //Création d'une entité pour la carte de contôle des pas à pas
L6470_init_t init[L6470DAISYCHAINSIZE] = {
    /* First Motor. */
    {
        12,                           /* Motor supply voltage in V. */
        200,                           /* Min number of steps per revolution for the motor. */
        4,                           /* Max motor phase voltage in A. */
        7,                          /* Max motor phase voltage in V. */
        300,                         /* Motor initial speed [step/s]. */
        247.4,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        1500.0,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        992.0,                         /* Motor maximum speed [step/s]. */
        0.0,                           /* Motor minimum speed [step/s]. */
        602.7,                         /* Motor full-step speed threshold [step/s]. */
        5,                          /* Holding kval [V]. */
        5,                          /* Constant speed kval [V]. */
        5,                          /* Acceleration starting kval [V]. */
        5,                          /* Deceleration starting kval [V]. */
        61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        392.1569e-6,                   /* Start slope [s/step]. */
        643.1372e-6,                   /* Acceleration final slope [s/step]. */
        643.1372e-6,                   /* Deceleration final slope [s/step]. */
        0,                             /* Thermal compensation factor (range [0, 15]). */
        5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        5*1000*1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
        0xFF,                          /* Alarm conditions enable. */
        0x2E88                         /* Ic configuration. */
    },

    /* Second Motor. */
    {
        12,                           /* Motor supply voltage in V. */
        200,                           /* Min number of steps per revolution for the motor. */
        4,                           /* Max motor phase voltage in A. */
        7,                          /* Max motor phase voltage in V. */
        300,                         /* Motor initial speed [step/s]. */
        251.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        1500.0,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        992.0,                         /* Motor maximum speed [step/s]. */
        0.0,                           /* Motor minimum speed [step/s]. */
        602.7,                         /* Motor full-step speed threshold [step/s]. */
        5,                          /* Holding kval [V]. */
        5,                          /* Constant speed kval [V]. */
        5,                          /* Acceleration starting kval [V]. */
        5,                          /* Deceleration starting kval [V]. */
        61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        392.1569e-6,                   /* Start slope [s/step]. */
        643.1372e-6,                   /* Acceleration final slope [s/step]. */
        643.1372e-6,                   /* Deceleration final slope [s/step]. */
        0,                             /* Thermal compensation factor (range [0, 15]). */
        5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        5*1000*1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
        0xFF,                          /* Alarm conditions enable. */
        0x2E88                         /* Ic configuration. */
    }
};

L6470 **motors; //Instance des moteurs

DigitalOut led(LED2);
Serial pc(USBTX, USBRX); // tx, rx
DevSPI dev_spi(D11, D12, D3);


//Connections codeuses
//Nucleo 401re
InterruptIn ENCAL(D9);
InterruptIn ENCAJ(D8);
InterruptIn ENCBL(D6);
InterruptIn ENCBJ(D5);
/*//Nucelo 746zg
InterruptIn ENCAL(D8);
InterruptIn ENCAJ(D7);
InterruptIn ENCBL(D4);
InterruptIn ENCBJ(D0);*/

volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B

void init_hardware()
{
    pc.baud(2000000); //Initialisation de l'USART pc 

    /* Initializing Motor Control Expansion Board. */
    x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
    motors = x_nucleo_ihm02a1->get_components();
    
    ENCAL.mode(PullUp); //Initialisation des codeuses
    ENCAJ.mode(PullUp);
    ENCBL.mode(PullUp);
    ENCBJ.mode(PullUp);

    ENCAL.rise(&updateEncoderA);
    ENCAL.fall(&updateEncoderA);
    ENCAJ.rise(&updateEncoderA);
    ENCAJ.fall(&updateEncoderA);

    ENCBL.rise(&updateEncoderB);
    ENCBL.fall(&updateEncoderB);
    ENCBJ.rise(&updateEncoderB);
    ENCBJ.fall(&updateEncoderB);

}

void set_PWM_moteur_D(int PWM)
{
    if (!moteurs_arret) {
        if (PWM > PWM_MAX) {
            motors[0]->prepare_run(StepperMotor::BWD, PWM_MAX); //BWD = backward , FWD = forward , la vitesse doit etre positive
        } else if (PWM <-PWM_MAX) {
            motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
        } else if (PWM > 0) {
            motors[0]->prepare_run(StepperMotor::BWD, PWM);
        } else if (PWM < 0) {
            motors[0]->prepare_run(StepperMotor::FWD, -PWM);
        } else if (PWM == 0) {
            motors[0]->prepare_run(StepperMotor::BWD, 0);
        }
    } else {
        motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
    }
    x_nucleo_ihm02a1->perform_prepared_actions();
}

void set_PWM_moteur_G(int PWM)
{

    if (!moteurs_arret) {
        if (PWM > PWM_MAX) {
            motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
        } else if (PWM <-PWM_MAX) {
            motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
        } else if (PWM > 0) {
            motors[1]->prepare_run(StepperMotor::FWD, PWM);
        } else if (PWM < 0) {
            motors[1]->prepare_run(StepperMotor::BWD, -PWM);
        } else if (PWM == 0) {
            motors[1]->prepare_run(StepperMotor::BWD, 0);
        }
    } else {
        motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
    }
    x_nucleo_ihm02a1->perform_prepared_actions();
}

void set_step_moteur_D(int steps)
{
    if (!moteurs_arret) {
        if (1) {
            motors[0]->prepare_move(StepperMotor::BWD, steps); //BWD = backward , FWD = forward , la vitesse doit etre positive
        /*} else if (PWM <-PWM_MAX) {
            motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
        } else if (PWM > 0) {
            motors[0]->prepare_run(StepperMotor::BWD, PWM);
        } else if (PWM < 0) {
            motors[0]->prepare_run(StepperMotor::FWD, -PWM);
        } else if (PWM == 0) {
            motors[0]->prepare_run(StepperMotor::BWD, 0);
        */}
    /*} else {
        motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main*/
    }
    x_nucleo_ihm02a1->perform_prepared_actions();
}
/*
void set_step_moteur_G(int PWM)
{

    if (!moteurs_arret) {
        if (PWM > PWM_MAX) {
            motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
        } else if (PWM <-PWM_MAX) {
            motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
        } else if (PWM > 0) {
            motors[1]->prepare_run(StepperMotor::FWD, PWM);
        } else if (PWM < 0) {
            motors[1]->prepare_run(StepperMotor::BWD, -PWM);
        } else if (PWM == 0) {
            motors[1]->prepare_run(StepperMotor::BWD, 0);
        }
    } else {
        motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
    }
    x_nucleo_ihm02a1->perform_prepared_actions();
}*/

long int get_nbr_tick_D()
{
    return encoderValueA;
}

long int get_nbr_tick_G()
{
    return encoderValueB;
}

void attente_synchro()
{
    //structute du temps d'attente de l'asservissement 10ms
    wait(0.010);
}

void motors_stop()
{
    moteurs_arret=1;
    motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
    motors[1]->prepare_hard_hiz(); 
    x_nucleo_ihm02a1->perform_prepared_actions();
}

void motors_on()
{
    moteurs_arret=0;
}


void allumer_del()
{
    led = 1;
}

void eteindre_del()
{
    led = 0;
}

void delay_ms()
{
}

void allumer_autres_del()
{
}

void eteindre_autres_del()
{
}
void toggle_autres_del() {}

void set_all_led()
{

}


volatile int lastEncodedA = 0;
long lastencoderValueA = 0;
int lastMSBA = 0;
int lastLSBA = 0;

void updateEncoderA()
{
    int MSBA = ENCAL.read(); //MSB = most significant bit
    int LSBA = ENCAJ.read(); //LSB = least significant bit

    int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
    int sumA  = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value

    if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
    if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;

    lastEncodedA = encodedA; //store this value for next time
}


volatile int lastEncodedB = 0;
long lastencoderValueB = 0;
int lastMSBB = 0;
int lastLSBB = 0;

void updateEncoderB()
{
    int MSBB = ENCBL.read(); //MSB = most significant bit
    int LSBB = ENCBJ.read(); //LSB = least significant bit

    int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
    int sumB  = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value

    if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
    if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;

    lastEncodedB = encodedB; //store this value for next time
}




/*void asservissement(){
    long int tick_D = get_nbr_tick_D();
    long int tick_G = get_nbr_tick_G();
    
    long int tick_D_passe = tick_D-tick_prec_D;
    long int tick_G_passe = tick_G-tick_prec_G;
    
    tick_prec_D=tick_D;
    tick_prec_G=tick_G;
    
    float vitesse_codeuse_D = tick_D_passe;
    float vitesse_codeuse_G = tick_G_passe;
    
    float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
    float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
    
    somme_erreur_D += erreur_D;
    somme_erreur_G += erreur_G;
    
    float delta_erreur_D = erreur_D-erreur_precedente_D;
    float delta_erreur_G = erreur_G-erreur_precedente_G;
    
    erreur_precedente_G = erreur_G;
    erreur_precedente_D = erreur_D;
    
    float cmd_D = Kp*erreur_D+Ki*somme_erreur_D + Kd*delta_erreur_D;
    float cmd_G = Kp*erreur_G+Ki*somme_erreur_G + Kd*delta_erreur_G;
    
    if (cmd_G <0){
        cmd_G = 0;
    }
    if (cmd_G > 500){
        cmd_G = 500;
    }
    if (cmd_D <0){
        cmd_D = 0;
    }
    if (cmd_D > 500){
        cmd_D = 500;
    }
    commande_vitesse(cmd_G,cmd_D);
    //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
    //printf("%f\n",vitesse_codeuse_G);
    //printf("oui");
}*/

void debugEncoder()
{
    printf("tick_D : %ld, tick_G : %ld\n", get_nbr_tick_D(),get_nbr_tick_G());
}
long int get_position_G(){
    /* Getting the current position. */
    long int position = motors[1]->get_position();
    return position;
    /* Printing to the console. */
    //printf("--> Getting the current position: %d\r\n", position);
    
}
long int get_position_D(){
    /* Getting the current position. */
    long int position = motors[0]->get_position();
    
    /* Printing to the console. */
    //printf("--> Getting the current position: %d\r\n", position);
    return position;
    
}