Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
Diff: hardware.cpp
- Revision:
- 0:38dbe2988e77
- Child:
- 1:0d76bc4e1aea
diff -r 000000000000 -r 38dbe2988e77 hardware.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hardware.cpp Wed Apr 17 09:05:21 2019 +0000 @@ -0,0 +1,283 @@ +#define _POSIX_C_SOURCE 199309L +#include "mbed.h" +#include "reglages.h" +#include "hardware.h" +#include "DevSPI.h" +#include "XNucleoIHM02A1.h" + +// PWM_MAX est définit dans réglage; +bool moteurs_arret = false; + + +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. */ + { + 4.08, /* Motor supply voltage in V. */ + 200, /* Min number of steps per revolution for the motor. */ + 7.5, /* Max motor phase voltage in A. */ + 7.06, /* Max motor phase voltage in V. */ + 300.0, /* Motor initial speed [step/s]. */ + 478, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */ + 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */ + 1500.0, /* Motor maximum speed [step/s]. */ + 0.0, /* Motor minimum speed [step/s]. */ + 602.7, /* Motor full-step speed threshold [step/s]. */ + 3.06, /* Holding kval [V]. */ + 3.06, /* Constant speed kval [V]. */ + 3.06, /* Acceleration starting kval [V]. */ + 3.06, /* 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]). */ + 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */ + 3.06 * 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. */ + { + 4.08, /* Motor supply voltage in V. */ + 200, /* Min number of steps per revolution for the motor. */ + 7.5, /* Max motor phase voltage in A. */ + 7.06, /* Max motor phase voltage in V. */ + 300.0, /* Motor initial speed [step/s]. */ + 500, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */ + 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */ + 1500.0, /* Motor maximum speed [step/s]. */ + 0.0, /* Motor minimum speed [step/s]. */ + 602.7, /* Motor full-step speed threshold [step/s]. */ + 3.06, /* Holding kval [V]. */ + 3.06, /* Constant speed kval [V]. */ + 3.06, /* Acceleration starting kval [V]. */ + 3.06, /* 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]). */ + 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */ + 3.06 * 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); + + + +InterruptIn ENCBL(D5); +InterruptIn ENCBJ(D6); + + +/*void init_hardware() +{ + pc.baud(115200); //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(); + + +}*/ + +volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B + +void init_hardware() +{ + pc.baud(115200); //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_G(int steps) +{ + if (!moteurs_arret) { + if (steps<0) { + motors[1]->prepare_move(StepperMotor::BWD, steps); //BWD = backward , BWD = backward , la vitesse doit etre positive + } + else if(steps >0){ + motors[1]->prepare_move(StepperMotor::FWD, steps); //BWD = backward , FWD = forward , la vitesse doit etre positive + } + 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) +{ + int steps_modulo_128 = steps/128; + if (!moteurs_arret) { + if (steps<0) { + motors[0]->prepare_move(StepperMotor::BWD, steps_modulo_128*128); //BWD = backward , BWD = backward , la vitesse doit etre positive + } + else if(steps >0){ + motors[0]->prepare_move(StepperMotor::FWD, steps_modulo_128*128); //BWD = backward , FWD = forward , la vitesse doit etre positive + } + else { + motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main*/ + } + } + x_nucleo_ihm02a1->perform_prepared_actions(); +} +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; + +} +long int get_nbr_tick_G() +{ + return encoderValueB; +} + +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 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() +{ + +} + +