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