Corentin Courtot / Mbed 2 deprecated Asserve

asser

Dependencies:   mbed X_NUCLEO_IHM02A1

hardware.cpp@5:bbca34b60427, 2019-05-19 (annotated)

Committer:
Coconara
Date:
Sun May 19 20:09:32 2019 +0000
Revision:
5:bbca34b60427
Parent:
4:deef042e9c02 
asser

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