Guillaume Chauvon
asser1
Diff: hardware.cpp
- Revision:
- 0:6ca63d45f0ee
- Child:
- 2:5764f89a27f6
diff -r 000000000000 -r 6ca63d45f0ee hardware.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/hardware.cpp Tue Dec 11 19:12:55 2018 +0000
@@ -0,0 +1,254 @@
+#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. */
+ {
+ 9.0, /* Motor supply voltage in V. */
+ 400, /* Min number of steps per revolution for the motor. */
+ 1.7, /* Max motor phase voltage in A. */
+ 3.06, /* Max motor phase voltage in V. */
+ 300.0, /* Motor initial speed [step/s]. */
+ 500.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]. */
+ 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. */
+ {
+ 9.0, /* Motor supply voltage in V. */
+ 400, /* Min number of steps per revolution for the motor. */
+ 1.7, /* Max motor phase voltage in A. */
+ 3.06, /* Max motor phase voltage in V. */
+ 300.0, /* Motor initial speed [step/s]. */
+ 500.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]. */
+ 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);
+
+
+//Connections codeuses
+InterruptIn ENCAL(D6);
+InterruptIn ENCAJ(D5);
+InterruptIn ENCBL(D8);
+InterruptIn ENCBJ(D7);
+
+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(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();
+}
+
+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 debugEncoder()
+{
+ printf("codeuse droite : %d, codeuse gauche : %d\n", lastEncodedB, lastEncodedA);
+}
\ No newline at end of file