Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
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?
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 | 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 | } |