Jimmy Maingam / Mbed 2 deprecated odometrieDecembre

assert1

Dependencies:   mbed X_NUCLEO_IHM02A1

hardware.cpp@2:977799d72329, 2019-02-23 (annotated)

Committer:
Pierr
Date:
Sat Feb 23 16:30:08 2019 +0000
Revision:
2:977799d72329
Parent:
0:6ca63d45f0ee
Child:
3:06cbe2f6c494
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Coconara 0:6ca63d45f0ee 1 #define _POSIX_C_SOURCE 199309L
Coconara 0:6ca63d45f0ee 2 #include "mbed.h"
Coconara 0:6ca63d45f0ee 3 #include "reglages.h"
Coconara 0:6ca63d45f0ee 4 #include "hardware.h"
Coconara 0:6ca63d45f0ee 5 #include "DevSPI.h"
Coconara 0:6ca63d45f0ee 6 #include "XNucleoIHM02A1.h"
Coconara 0:6ca63d45f0ee 7
Coconara 0:6ca63d45f0ee 8 // PWM_MAX est définit dans réglage;
Coconara 0:6ca63d45f0ee 9 bool moteurs_arret = false;
Coconara 0:6ca63d45f0ee 10
Pierr 2:977799d72329 11 DigitalIn tirette(D9);
Pierr 2:977799d72329 12
Coconara 0:6ca63d45f0ee 13
Coconara 0:6ca63d45f0ee 14 XNucleoIHM02A1 *x_nucleo_ihm02a1; //Création d'une entité pour la carte de contôle des pas à pas
Coconara 0:6ca63d45f0ee 15 L6470_init_t init[L6470DAISYCHAINSIZE] = {
Coconara 0:6ca63d45f0ee 16 /* First Motor. */
Pierr 2:977799d72329 17 {
Pierr 2:977799d72329 18 12.0, /* Motor supply voltage in V. */
Coconara 0:6ca63d45f0ee 19 400, /* Min number of steps per revolution for the motor. */
Pierr 2:977799d72329 20 7.5, /* Max motor phase voltage in A. */
Pierr 2:977799d72329 21 7.06, /* Max motor phase voltage in V. */
Coconara 0:6ca63d45f0ee 22 300.0, /* Motor initial speed [step/s]. */
Coconara 0:6ca63d45f0ee 23 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
Pierr 2:977799d72329 24 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
Pierr 2:977799d72329 25 PWM_MAX, /* Motor maximum speed [step/s]. */
Pierr 2:977799d72329 26 PWM_MIN, /* Motor minimum speed [step/s]. */
Coconara 0:6ca63d45f0ee 27 602.7, /* Motor full-step speed threshold [step/s]. */
Coconara 0:6ca63d45f0ee 28 3.06, /* Holding kval [V]. */
Coconara 0:6ca63d45f0ee 29 3.06, /* Constant speed kval [V]. */
Coconara 0:6ca63d45f0ee 30 3.06, /* Acceleration starting kval [V]. */
Coconara 0:6ca63d45f0ee 31 3.06, /* Deceleration starting kval [V]. */
Coconara 0:6ca63d45f0ee 32 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Coconara 0:6ca63d45f0ee 33 392.1569e-6, /* Start slope [s/step]. */
Coconara 0:6ca63d45f0ee 34 643.1372e-6, /* Acceleration final slope [s/step]. */
Coconara 0:6ca63d45f0ee 35 643.1372e-6, /* Deceleration final slope [s/step]. */
Coconara 0:6ca63d45f0ee 36 0, /* Thermal compensation factor (range [0, 15]). */
Coconara 0:6ca63d45f0ee 37 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Pierr 2:977799d72329 38 32, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Coconara 0:6ca63d45f0ee 39 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
Coconara 0:6ca63d45f0ee 40 0xFF, /* Alarm conditions enable. */
Coconara 0:6ca63d45f0ee 41 0x2E88 /* Ic configuration. */
Coconara 0:6ca63d45f0ee 42 },
Coconara 0:6ca63d45f0ee 43
Coconara 0:6ca63d45f0ee 44 /* Second Motor. */
Coconara 0:6ca63d45f0ee 45 {
Pierr 2:977799d72329 46 12.0, /* Motor supply voltage in V. */
Coconara 0:6ca63d45f0ee 47 400, /* Min number of steps per revolution for the motor. */
Pierr 2:977799d72329 48 7.5, /* Max motor phase voltage in A. */
Pierr 2:977799d72329 49 7.06, /* Max motor phase voltage in V. */
Coconara 0:6ca63d45f0ee 50 300.0, /* Motor initial speed [step/s]. */
Coconara 0:6ca63d45f0ee 51 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
Pierr 2:977799d72329 52 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
Pierr 2:977799d72329 53 PWM_MAX, /* Motor maximum speed [step/s]. */
Pierr 2:977799d72329 54 PWM_MIN, /* Motor minimum speed [step/s]. */
Coconara 0:6ca63d45f0ee 55 602.7, /* Motor full-step speed threshold [step/s]. */
Coconara 0:6ca63d45f0ee 56 3.06, /* Holding kval [V]. */
Coconara 0:6ca63d45f0ee 57 3.06, /* Constant speed kval [V]. */
Coconara 0:6ca63d45f0ee 58 3.06, /* Acceleration starting kval [V]. */
Coconara 0:6ca63d45f0ee 59 3.06, /* Deceleration starting kval [V]. */
Coconara 0:6ca63d45f0ee 60 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Coconara 0:6ca63d45f0ee 61 392.1569e-6, /* Start slope [s/step]. */
Coconara 0:6ca63d45f0ee 62 643.1372e-6, /* Acceleration final slope [s/step]. */
Coconara 0:6ca63d45f0ee 63 643.1372e-6, /* Deceleration final slope [s/step]. */
Coconara 0:6ca63d45f0ee 64 0, /* Thermal compensation factor (range [0, 15]). */
Coconara 0:6ca63d45f0ee 65 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Pierr 2:977799d72329 66 32, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Coconara 0:6ca63d45f0ee 67 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
Coconara 0:6ca63d45f0ee 68 0xFF, /* Alarm conditions enable. */
Coconara 0:6ca63d45f0ee 69 0x2E88 /* Ic configuration. */
Coconara 0:6ca63d45f0ee 70 }
Coconara 0:6ca63d45f0ee 71 };
Coconara 0:6ca63d45f0ee 72
Coconara 0:6ca63d45f0ee 73 L6470 **motors; //Instance des moteurs
Coconara 0:6ca63d45f0ee 74
Coconara 0:6ca63d45f0ee 75 DigitalOut led(LED2);
Coconara 0:6ca63d45f0ee 76 Serial pc(USBTX, USBRX); // tx, rx
Pierr 2:977799d72329 77 DevSPI dev_spi(D11, D12, D13); //pour la F746ZG, connecter D13 et D3 par un fil
Coconara 0:6ca63d45f0ee 78
Coconara 0:6ca63d45f0ee 79
Coconara 0:6ca63d45f0ee 80 //Connections codeuses
Coconara 0:6ca63d45f0ee 81 InterruptIn ENCAL(D6);
Coconara 0:6ca63d45f0ee 82 InterruptIn ENCAJ(D5);
Coconara 0:6ca63d45f0ee 83 InterruptIn ENCBL(D8);
Coconara 0:6ca63d45f0ee 84 InterruptIn ENCBJ(D7);
Coconara 0:6ca63d45f0ee 85
Coconara 0:6ca63d45f0ee 86 volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
Coconara 0:6ca63d45f0ee 87 volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B
Coconara 0:6ca63d45f0ee 88
Coconara 0:6ca63d45f0ee 89 void init_hardware()
Coconara 0:6ca63d45f0ee 90 {
Coconara 0:6ca63d45f0ee 91 pc.baud(115200); //Initialisation de l'USART pc
Coconara 0:6ca63d45f0ee 92
Coconara 0:6ca63d45f0ee 93 /* Initializing Motor Control Expansion Board. */
Coconara 0:6ca63d45f0ee 94 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
Coconara 0:6ca63d45f0ee 95 motors = x_nucleo_ihm02a1->get_components();
Coconara 0:6ca63d45f0ee 96
Coconara 0:6ca63d45f0ee 97 ENCAL.mode(PullUp); //Initialisation des codeuses
Coconara 0:6ca63d45f0ee 98 ENCAJ.mode(PullUp);
Coconara 0:6ca63d45f0ee 99 ENCBL.mode(PullUp);
Coconara 0:6ca63d45f0ee 100 ENCBJ.mode(PullUp);
Coconara 0:6ca63d45f0ee 101
Coconara 0:6ca63d45f0ee 102 ENCAL.rise(&updateEncoderA);
Coconara 0:6ca63d45f0ee 103 ENCAL.fall(&updateEncoderA);
Coconara 0:6ca63d45f0ee 104 ENCAJ.rise(&updateEncoderA);
Coconara 0:6ca63d45f0ee 105 ENCAJ.fall(&updateEncoderA);
Coconara 0:6ca63d45f0ee 106
Coconara 0:6ca63d45f0ee 107 ENCBL.rise(&updateEncoderB);
Coconara 0:6ca63d45f0ee 108 ENCBL.fall(&updateEncoderB);
Coconara 0:6ca63d45f0ee 109 ENCBJ.rise(&updateEncoderB);
Coconara 0:6ca63d45f0ee 110 ENCBJ.fall(&updateEncoderB);
Coconara 0:6ca63d45f0ee 111
Coconara 0:6ca63d45f0ee 112 }
Coconara 0:6ca63d45f0ee 113
Coconara 0:6ca63d45f0ee 114 void set_PWM_moteur_D(int PWM)
Coconara 0:6ca63d45f0ee 115 {
Coconara 0:6ca63d45f0ee 116 if (!moteurs_arret) {
Coconara 0:6ca63d45f0ee 117 if (PWM > PWM_MAX) {
Coconara 0:6ca63d45f0ee 118 motors[0]->prepare_run(StepperMotor::BWD, PWM_MAX); //BWD = backward , FWD = forward , la vitesse doit etre positive
Coconara 0:6ca63d45f0ee 119 } else if (PWM <-PWM_MAX) {
Coconara 0:6ca63d45f0ee 120 motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
Coconara 0:6ca63d45f0ee 121 } else if (PWM > 0) {
Coconara 0:6ca63d45f0ee 122 motors[0]->prepare_run(StepperMotor::BWD, PWM);
Coconara 0:6ca63d45f0ee 123 } else if (PWM < 0) {
Coconara 0:6ca63d45f0ee 124 motors[0]->prepare_run(StepperMotor::FWD, -PWM);
Coconara 0:6ca63d45f0ee 125 } else if (PWM == 0) {
Coconara 0:6ca63d45f0ee 126 motors[0]->prepare_run(StepperMotor::BWD, 0);
Coconara 0:6ca63d45f0ee 127 }
Coconara 0:6ca63d45f0ee 128 } else {
Coconara 0:6ca63d45f0ee 129 motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
Coconara 0:6ca63d45f0ee 130 }
Coconara 0:6ca63d45f0ee 131 x_nucleo_ihm02a1->perform_prepared_actions();
Coconara 0:6ca63d45f0ee 132 }
Coconara 0:6ca63d45f0ee 133
Coconara 0:6ca63d45f0ee 134 void set_PWM_moteur_G(int PWM)
Coconara 0:6ca63d45f0ee 135 {
Coconara 0:6ca63d45f0ee 136
Coconara 0:6ca63d45f0ee 137 if (!moteurs_arret) {
Coconara 0:6ca63d45f0ee 138 if (PWM > PWM_MAX) {
Coconara 0:6ca63d45f0ee 139 motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
Coconara 0:6ca63d45f0ee 140 } else if (PWM <-PWM_MAX) {
Coconara 0:6ca63d45f0ee 141 motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
Coconara 0:6ca63d45f0ee 142 } else if (PWM > 0) {
Coconara 0:6ca63d45f0ee 143 motors[1]->prepare_run(StepperMotor::FWD, PWM);
Coconara 0:6ca63d45f0ee 144 } else if (PWM < 0) {
Coconara 0:6ca63d45f0ee 145 motors[1]->prepare_run(StepperMotor::BWD, -PWM);
Coconara 0:6ca63d45f0ee 146 } else if (PWM == 0) {
Coconara 0:6ca63d45f0ee 147 motors[1]->prepare_run(StepperMotor::BWD, 0);
Coconara 0:6ca63d45f0ee 148 }
Coconara 0:6ca63d45f0ee 149 } else {
Coconara 0:6ca63d45f0ee 150 motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
Coconara 0:6ca63d45f0ee 151 }
Coconara 0:6ca63d45f0ee 152 x_nucleo_ihm02a1->perform_prepared_actions();
Coconara 0:6ca63d45f0ee 153 }
Coconara 0:6ca63d45f0ee 154
Coconara 0:6ca63d45f0ee 155 long int get_nbr_tick_D()
Coconara 0:6ca63d45f0ee 156 {
Coconara 0:6ca63d45f0ee 157 return encoderValueA;
Coconara 0:6ca63d45f0ee 158 }
Coconara 0:6ca63d45f0ee 159
Coconara 0:6ca63d45f0ee 160 long int get_nbr_tick_G()
Coconara 0:6ca63d45f0ee 161 {
Coconara 0:6ca63d45f0ee 162 return encoderValueB;
Coconara 0:6ca63d45f0ee 163 }
Coconara 0:6ca63d45f0ee 164
Coconara 0:6ca63d45f0ee 165 void attente_synchro()
Coconara 0:6ca63d45f0ee 166 {
Coconara 0:6ca63d45f0ee 167 //structute du temps d'attente de l'asservissement 10ms
Coconara 0:6ca63d45f0ee 168 wait(0.010);
Coconara 0:6ca63d45f0ee 169 }
Coconara 0:6ca63d45f0ee 170
Coconara 0:6ca63d45f0ee 171 void motors_stop()
Coconara 0:6ca63d45f0ee 172 {
Coconara 0:6ca63d45f0ee 173 moteurs_arret=1;
Coconara 0:6ca63d45f0ee 174 motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
Coconara 0:6ca63d45f0ee 175 motors[1]->prepare_hard_hiz();
Coconara 0:6ca63d45f0ee 176 x_nucleo_ihm02a1->perform_prepared_actions();
Coconara 0:6ca63d45f0ee 177 }
Coconara 0:6ca63d45f0ee 178
Coconara 0:6ca63d45f0ee 179 void motors_on()
Coconara 0:6ca63d45f0ee 180 {
Coconara 0:6ca63d45f0ee 181 moteurs_arret=0;
Coconara 0:6ca63d45f0ee 182 }
Coconara 0:6ca63d45f0ee 183
Coconara 0:6ca63d45f0ee 184
Coconara 0:6ca63d45f0ee 185 void allumer_del()
Coconara 0:6ca63d45f0ee 186 {
Coconara 0:6ca63d45f0ee 187 led = 1;
Coconara 0:6ca63d45f0ee 188 }
Coconara 0:6ca63d45f0ee 189
Coconara 0:6ca63d45f0ee 190 void eteindre_del()
Coconara 0:6ca63d45f0ee 191 {
Coconara 0:6ca63d45f0ee 192 led = 0;
Coconara 0:6ca63d45f0ee 193 }
Coconara 0:6ca63d45f0ee 194
Coconara 0:6ca63d45f0ee 195 void delay_ms()
Coconara 0:6ca63d45f0ee 196 {
Coconara 0:6ca63d45f0ee 197 }
Coconara 0:6ca63d45f0ee 198
Coconara 0:6ca63d45f0ee 199 void allumer_autres_del()
Coconara 0:6ca63d45f0ee 200 {
Coconara 0:6ca63d45f0ee 201 }
Coconara 0:6ca63d45f0ee 202
Coconara 0:6ca63d45f0ee 203 void eteindre_autres_del()
Coconara 0:6ca63d45f0ee 204 {
Coconara 0:6ca63d45f0ee 205 }
Coconara 0:6ca63d45f0ee 206 void toggle_autres_del() {}
Coconara 0:6ca63d45f0ee 207
Coconara 0:6ca63d45f0ee 208 void set_all_led()
Coconara 0:6ca63d45f0ee 209 {
Coconara 0:6ca63d45f0ee 210
Coconara 0:6ca63d45f0ee 211 }
Coconara 0:6ca63d45f0ee 212
Coconara 0:6ca63d45f0ee 213
Coconara 0:6ca63d45f0ee 214 volatile int lastEncodedA = 0;
Coconara 0:6ca63d45f0ee 215 long lastencoderValueA = 0;
Coconara 0:6ca63d45f0ee 216 int lastMSBA = 0;
Coconara 0:6ca63d45f0ee 217 int lastLSBA = 0;
Coconara 0:6ca63d45f0ee 218
Coconara 0:6ca63d45f0ee 219 void updateEncoderA()
Coconara 0:6ca63d45f0ee 220 {
Coconara 0:6ca63d45f0ee 221 int MSBA = ENCAL.read(); //MSB = most significant bit
Coconara 0:6ca63d45f0ee 222 int LSBA = ENCAJ.read(); //LSB = least significant bit
Coconara 0:6ca63d45f0ee 223
Coconara 0:6ca63d45f0ee 224 int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
Coconara 0:6ca63d45f0ee 225 int sumA = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value
Coconara 0:6ca63d45f0ee 226
Coconara 0:6ca63d45f0ee 227 if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
Coconara 0:6ca63d45f0ee 228 if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;
Coconara 0:6ca63d45f0ee 229
Coconara 0:6ca63d45f0ee 230 lastEncodedA = encodedA; //store this value for next time
Coconara 0:6ca63d45f0ee 231 }
Coconara 0:6ca63d45f0ee 232
Coconara 0:6ca63d45f0ee 233
Coconara 0:6ca63d45f0ee 234 volatile int lastEncodedB = 0;
Coconara 0:6ca63d45f0ee 235 long lastencoderValueB = 0;
Coconara 0:6ca63d45f0ee 236 int lastMSBB = 0;
Coconara 0:6ca63d45f0ee 237 int lastLSBB = 0;
Coconara 0:6ca63d45f0ee 238
Coconara 0:6ca63d45f0ee 239 void updateEncoderB()
Coconara 0:6ca63d45f0ee 240 {
Coconara 0:6ca63d45f0ee 241 int MSBB = ENCBL.read(); //MSB = most significant bit
Coconara 0:6ca63d45f0ee 242 int LSBB = ENCBJ.read(); //LSB = least significant bit
Coconara 0:6ca63d45f0ee 243
Coconara 0:6ca63d45f0ee 244 int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
Coconara 0:6ca63d45f0ee 245 int sumB = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
Coconara 0:6ca63d45f0ee 246
Coconara 0:6ca63d45f0ee 247 if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
Coconara 0:6ca63d45f0ee 248 if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
Coconara 0:6ca63d45f0ee 249
Coconara 0:6ca63d45f0ee 250 lastEncodedB = encodedB; //store this value for next time
Coconara 0:6ca63d45f0ee 251 }
Coconara 0:6ca63d45f0ee 252
Coconara 0:6ca63d45f0ee 253 void debugEncoder()
Coconara 0:6ca63d45f0ee 254 {
Coconara 0:6ca63d45f0ee 255 printf("codeuse droite : %d, codeuse gauche : %d\n", lastEncodedB, lastEncodedA);
Coconara 0:6ca63d45f0ee 256 }