Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

hardware.cpp@2:3066e614372f, 2019-05-08 (annotated)

Committer:
GuillaumeCH
Date:
Wed May 08 21:19:10 2019 +0000
Revision:
2:3066e614372f
Parent:
1:0d76bc4e1aea
Child:
3:d38aa400d5e7
a

Who changed what in which revision?

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