hardware.cpp

00001 #define _POSIX_C_SOURCE 199309L
00002 #include "mbed.h"
00003 #include "reglages.h"
00004 #include "hardware.h"
00005 #include "DevSPI.h"
00006 #include "XNucleoIHM02A1.h"
00007 
00008 // PWM_MAX est définit dans réglage;
00009 bool moteurs_arret = false;
00010 
00011 DigitalIn tirette(PB_8);
00012 
00013 
00014 XNucleoIHM02A1 *x_nucleo_ihm02a1; //Création d'une entité pour la carte de contôle des pas à pas
00015 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00016     /* First Motor. */
00017      {
00018         4.08,                           /* Motor supply voltage in V. */ //4.08
00019         400,                           /* Min number of steps per revolution for the motor. */
00020         3,                           /* Max motor phase voltage in A. */ 
00021         7.06,                          /* Max motor phase voltage in V. */
00022         300.0,                         /* Motor initial speed [step/s]. */
00023         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00024         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00025         PWM_MAX,                         /* Motor maximum speed [step/s]. */
00026         PWM_MIN,                           /* Motor minimum speed [step/s]. */
00027         602.7,                         /* Motor full-step speed threshold [step/s]. */
00028         3.06,                          /* Holding kval [V]. */
00029         3.06,                          /* Constant speed kval [V]. */
00030         3.06,                          /* Acceleration starting kval [V]. */
00031         3.06,                          /* Deceleration starting kval [V]. */
00032         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00033         392.1569e-6,                   /* Start slope [s/step]. */
00034         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00035         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00036         0,                             /* Thermal compensation factor (range [0, 15]). */
00037         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00038         32,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00039         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00040         0xFF,                          /* Alarm conditions enable. */
00041         0x2E88                         /* Ic configuration. */
00042     },
00043 
00044     /* Second Motor. */
00045     {
00046         4.08,                           /* Motor supply voltage in V. */
00047         400,                           /* Min number of steps per revolution for the motor. */
00048         3,                           /* Max motor phase voltage in A. */ //7.5 
00049         7.06,                          /* Max motor phase voltage in V. */
00050         300.0,                         /* Motor initial speed [step/s]. */
00051         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00052         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00053         PWM_MAX,                         /* Motor maximum speed [step/s]. */
00054         PWM_MIN,                           /* Motor minimum speed [step/s]. */
00055         602.7,                         /* Motor full-step speed threshold [step/s]. */
00056         3.06,                          /* Holding kval [V]. */
00057         3.06,                          /* Constant speed kval [V]. */
00058         3.06,                          /* Acceleration starting kval [V]. */
00059         3.06,                          /* Deceleration starting kval [V]. */
00060         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00061         392.1569e-6,                   /* Start slope [s/step]. */
00062         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00063         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00064         0,                             /* Thermal compensation factor (range [0, 15]). */
00065         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00066         32,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00067         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00068         0xFF,                          /* Alarm conditions enable. */
00069         0x2E88                         /* Ic configuration. */
00070     }
00071 };
00072 
00073 L6470 **motors; //Instance des moteurs
00074 
00075 DigitalOut led(LED2);
00076 Serial pc(USBTX, USBRX); // tx, rx
00077 DevSPI dev_spi(D11, D12, D3); //pour la F746ZG, connecter D13 et D3 par un fil 
00078 
00079 
00080 //Connections codeuses
00081 InterruptIn ENCAL(D6);
00082 InterruptIn ENCAJ(D5);
00083 InterruptIn ENCBL(D8);
00084 InterruptIn ENCBJ(D7);
00085 
00086 volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
00087 volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B
00088 
00089 void init_hardware()
00090 {
00091     pc.baud(115200); //Initialisation de l'USART pc 
00092 
00093     /* Initializing Motor Control Expansion Board. */
00094     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00095     motors = x_nucleo_ihm02a1->get_components();
00096     
00097     ENCAL.mode(PullUp); //Initialisation des codeuses
00098     ENCAJ.mode(PullUp);
00099     ENCBL.mode(PullUp);
00100     ENCBJ.mode(PullUp);
00101 
00102     ENCAL.rise(&updateEncoderA);
00103     ENCAL.fall(&updateEncoderA);
00104     ENCAJ.rise(&updateEncoderA);
00105     ENCAJ.fall(&updateEncoderA);
00106 
00107     ENCBL.rise(&updateEncoderB);
00108     ENCBL.fall(&updateEncoderB);
00109     ENCBJ.rise(&updateEncoderB);
00110     ENCBJ.fall(&updateEncoderB);
00111 
00112 }
00113 
00114 void set_PWM_moteur_D(int PWM)
00115 {
00116     if (!moteurs_arret) {
00117         if (PWM > PWM_MAX) {
00118             motors[0]->prepare_run(StepperMotor::BWD, PWM_MAX); //BWD = backward , FWD = forward , la vitesse doit etre positive
00119         } else if (PWM <-PWM_MAX) {
00120             motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
00121         } else if (PWM > 0) {
00122             motors[0]->prepare_run(StepperMotor::BWD, PWM);
00123         } else if (PWM < 0) {
00124             motors[0]->prepare_run(StepperMotor::FWD, -PWM);
00125         } else if (PWM == 0) {
00126             motors[0]->prepare_run(StepperMotor::BWD, 0);
00127         }
00128     } else {
00129         motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00130     }
00131     x_nucleo_ihm02a1->perform_prepared_actions();
00132 }
00133 
00134 void set_PWM_moteur_G(int PWM)
00135 {
00136 
00137     if (!moteurs_arret) {
00138         if (PWM > PWM_MAX) {
00139             motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
00140         } else if (PWM <-PWM_MAX) {
00141             motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
00142         } else if (PWM > 0) {
00143             motors[1]->prepare_run(StepperMotor::FWD, PWM);
00144         } else if (PWM < 0) {
00145             motors[1]->prepare_run(StepperMotor::BWD, -PWM);
00146         } else if (PWM == 0) {
00147             motors[1]->prepare_run(StepperMotor::BWD, 0);
00148         }
00149     } else {
00150         motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00151     }
00152     x_nucleo_ihm02a1->perform_prepared_actions();
00153 }
00154 
00155 long int get_nbr_tick_D()
00156 {
00157     return encoderValueA;
00158 }
00159 
00160 long int get_nbr_tick_G()
00161 {
00162     return encoderValueB;
00163 }
00164 
00165 void attente_synchro()
00166 {
00167     //structute du temps d'attente de l'asservissement 10ms
00168     wait(0.010);
00169 }
00170 
00171 void motors_stop()
00172 {
00173     moteurs_arret=1;
00174     motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
00175     motors[1]->prepare_hard_hiz(); 
00176     x_nucleo_ihm02a1->perform_prepared_actions();
00177 }
00178 
00179 void motors_on()
00180 {
00181     moteurs_arret=0;
00182 }
00183 
00184 
00185 void allumer_del()
00186 {
00187     led = 1;
00188 }
00189 
00190 void eteindre_del()
00191 {
00192     led = 0;
00193 }
00194 
00195 void delay_ms()
00196 {
00197 }
00198 
00199 void allumer_autres_del()
00200 {
00201 }
00202 
00203 void eteindre_autres_del()
00204 {
00205 }
00206 void toggle_autres_del() {}
00207 
00208 void set_all_led()
00209 {
00210 
00211 }
00212 
00213 
00214 volatile int lastEncodedA = 0;
00215 long lastencoderValueA = 0;
00216 int lastMSBA = 0;
00217 int lastLSBA = 0;
00218 
00219 void updateEncoderA()
00220 {
00221     int MSBA = ENCAL.read(); //MSB = most significant bit
00222     int LSBA = ENCAJ.read(); //LSB = least significant bit
00223 
00224     int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
00225     int sumA  = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value
00226 
00227     if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
00228     if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;
00229 
00230     lastEncodedA = encodedA; //store this value for next time
00231 }
00232 
00233 
00234 volatile int lastEncodedB = 0;
00235 long lastencoderValueB = 0;
00236 int lastMSBB = 0;
00237 int lastLSBB = 0;
00238 
00239 void updateEncoderB()
00240 {
00241     int MSBB = ENCBL.read(); //MSB = most significant bit
00242     int LSBB = ENCBJ.read(); //LSB = least significant bit
00243 
00244     int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
00245     int sumB  = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
00246 
00247     if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
00248     if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
00249 
00250     lastEncodedB = encodedB; //store this value for next time
00251 }
00252 
00253 void debugEncoder()
00254 {
00255     printf("codeuse droite : %d, codeuse gauche : %d\n", lastEncodedB, lastEncodedA);
00256 }