Corentin Courtot / Mbed 2 deprecated Asserve

asser

Dependencies:   mbed X_NUCLEO_IHM02A1

hardware.cpp@2:5764f89a27f6, 2019-04-17 (annotated)

Committer:
GuillaumeCH
Date:
Wed Apr 17 18:55:22 2019 +0000
Revision:
2:5764f89a27f6
Parent:
0:6ca63d45f0ee
Child:
3:1dba6eca01ad
p

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