Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

hardware.cpp@4:eac6746544fb, 2019-05-16 (annotated)

Committer:
GuillaumeCH
Date:
Thu May 16 09:10:16 2019 +0000
Revision:
4:eac6746544fb
Parent:
3:d38aa400d5e7
Child:
5:3638d7e7c5c1
oui

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 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 2:3066e614372f 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 2:3066e614372f 19 7, /* Max motor phase voltage in V. */
GuillaumeCH 1:0d76bc4e1aea 20 0, /* Motor initial speed [step/s]. */
GuillaumeCH 2:3066e614372f 21 500, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
GuillaumeCH 4:eac6746544fb 22 1500.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 2:3066e614372f 35 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
GuillaumeCH 2:3066e614372f 36 4.9*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 2:3066e614372f 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 2:3066e614372f 47 7, /* Max motor phase voltage in V. */
GuillaumeCH 1:0d76bc4e1aea 48 0, /* Motor initial speed [step/s]. */
GuillaumeCH 2:3066e614372f 49 490, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
GuillaumeCH 4:eac6746544fb 50 1500.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 2:3066e614372f 63 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
GuillaumeCH 2:3066e614372f 64 4.9*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 2:3066e614372f 78 //Connections codeuses
GuillaumeCH 2:3066e614372f 79 //Nucleo 401re
GuillaumeCH 2:3066e614372f 80 InterruptIn ENCAL(D9);
GuillaumeCH 2:3066e614372f 81 InterruptIn ENCAJ(D8);
GuillaumeCH 2:3066e614372f 82 InterruptIn ENCBL(D6);
GuillaumeCH 2:3066e614372f 83 InterruptIn ENCBJ(D5);
GuillaumeCH 3:d38aa400d5e7 84
GuillaumeCH 0:38dbe2988e77 85
GuillaumeCH 2:3066e614372f 86 volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
GuillaumeCH 0:38dbe2988e77 87 volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B
GuillaumeCH 0:38dbe2988e77 88
GuillaumeCH 0:38dbe2988e77 89 void init_hardware()
GuillaumeCH 0:38dbe2988e77 90 {
GuillaumeCH 2:3066e614372f 91 pc.baud(2000000); //Initialisation de l'USART pc
GuillaumeCH 0:38dbe2988e77 92
GuillaumeCH 0:38dbe2988e77 93 /* Initializing Motor Control Expansion Board. */
GuillaumeCH 0:38dbe2988e77 94 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
GuillaumeCH 0:38dbe2988e77 95 motors = x_nucleo_ihm02a1->get_components();
GuillaumeCH 0:38dbe2988e77 96
GuillaumeCH 2:3066e614372f 97 ENCAL.mode(PullUp); //Initialisation des codeuses
GuillaumeCH 2:3066e614372f 98 ENCAJ.mode(PullUp);
GuillaumeCH 0:38dbe2988e77 99 ENCBL.mode(PullUp);
GuillaumeCH 0:38dbe2988e77 100 ENCBJ.mode(PullUp);
GuillaumeCH 0:38dbe2988e77 101
GuillaumeCH 2:3066e614372f 102 ENCAL.rise(&updateEncoderA);
GuillaumeCH 2:3066e614372f 103 ENCAL.fall(&updateEncoderA);
GuillaumeCH 2:3066e614372f 104 ENCAJ.rise(&updateEncoderA);
GuillaumeCH 2:3066e614372f 105 ENCAJ.fall(&updateEncoderA);
GuillaumeCH 0:38dbe2988e77 106
GuillaumeCH 0:38dbe2988e77 107 ENCBL.rise(&updateEncoderB);
GuillaumeCH 0:38dbe2988e77 108 ENCBL.fall(&updateEncoderB);
GuillaumeCH 0:38dbe2988e77 109 ENCBJ.rise(&updateEncoderB);
GuillaumeCH 0:38dbe2988e77 110 ENCBJ.fall(&updateEncoderB);
GuillaumeCH 0:38dbe2988e77 111
GuillaumeCH 0:38dbe2988e77 112 }
GuillaumeCH 0:38dbe2988e77 113
GuillaumeCH 0:38dbe2988e77 114 void set_PWM_moteur_D(int PWM)
GuillaumeCH 0:38dbe2988e77 115 {
GuillaumeCH 0:38dbe2988e77 116 if (!moteurs_arret) {
GuillaumeCH 0:38dbe2988e77 117 if (PWM > PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 118 motors[0]->prepare_run(StepperMotor::BWD, PWM_MAX); //BWD = backward , FWD = forward , la vitesse doit etre positive
GuillaumeCH 0:38dbe2988e77 119 } else if (PWM <-PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 120 motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
GuillaumeCH 0:38dbe2988e77 121 } else if (PWM > 0) {
GuillaumeCH 0:38dbe2988e77 122 motors[0]->prepare_run(StepperMotor::BWD, PWM);
GuillaumeCH 0:38dbe2988e77 123 } else if (PWM < 0) {
GuillaumeCH 0:38dbe2988e77 124 motors[0]->prepare_run(StepperMotor::FWD, -PWM);
GuillaumeCH 0:38dbe2988e77 125 } else if (PWM == 0) {
GuillaumeCH 0:38dbe2988e77 126 motors[0]->prepare_run(StepperMotor::BWD, 0);
GuillaumeCH 0:38dbe2988e77 127 }
GuillaumeCH 0:38dbe2988e77 128 } else {
GuillaumeCH 0:38dbe2988e77 129 motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
GuillaumeCH 0:38dbe2988e77 130 }
GuillaumeCH 0:38dbe2988e77 131 x_nucleo_ihm02a1->perform_prepared_actions();
GuillaumeCH 0:38dbe2988e77 132 }
GuillaumeCH 0:38dbe2988e77 133
GuillaumeCH 0:38dbe2988e77 134 void set_PWM_moteur_G(int PWM)
GuillaumeCH 0:38dbe2988e77 135 {
GuillaumeCH 0:38dbe2988e77 136
GuillaumeCH 0:38dbe2988e77 137 if (!moteurs_arret) {
GuillaumeCH 0:38dbe2988e77 138 if (PWM > PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 139 motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
GuillaumeCH 0:38dbe2988e77 140 } else if (PWM <-PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 141 motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
GuillaumeCH 0:38dbe2988e77 142 } else if (PWM > 0) {
GuillaumeCH 0:38dbe2988e77 143 motors[1]->prepare_run(StepperMotor::FWD, PWM);
GuillaumeCH 0:38dbe2988e77 144 } else if (PWM < 0) {
GuillaumeCH 0:38dbe2988e77 145 motors[1]->prepare_run(StepperMotor::BWD, -PWM);
GuillaumeCH 0:38dbe2988e77 146 } else if (PWM == 0) {
GuillaumeCH 0:38dbe2988e77 147 motors[1]->prepare_run(StepperMotor::BWD, 0);
GuillaumeCH 0:38dbe2988e77 148 }
GuillaumeCH 0:38dbe2988e77 149 } else {
GuillaumeCH 0:38dbe2988e77 150 motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
GuillaumeCH 0:38dbe2988e77 151 }
GuillaumeCH 0:38dbe2988e77 152 x_nucleo_ihm02a1->perform_prepared_actions();
GuillaumeCH 0:38dbe2988e77 153 }
GuillaumeCH 0:38dbe2988e77 154
GuillaumeCH 2:3066e614372f 155
GuillaumeCH 2:3066e614372f 156 long int get_nbr_tick_D()
GuillaumeCH 2:3066e614372f 157 {
GuillaumeCH 2:3066e614372f 158 return encoderValueA;
GuillaumeCH 0:38dbe2988e77 159 }
GuillaumeCH 2:3066e614372f 160
GuillaumeCH 0:38dbe2988e77 161 long int get_nbr_tick_G()
GuillaumeCH 0:38dbe2988e77 162 {
GuillaumeCH 0:38dbe2988e77 163 return encoderValueB;
GuillaumeCH 0:38dbe2988e77 164 }
GuillaumeCH 0:38dbe2988e77 165
GuillaumeCH 0:38dbe2988e77 166 void attente_synchro()
GuillaumeCH 0:38dbe2988e77 167 {
GuillaumeCH 0:38dbe2988e77 168 //structute du temps d'attente de l'asservissement 10ms
GuillaumeCH 0:38dbe2988e77 169 wait(0.010);
GuillaumeCH 0:38dbe2988e77 170 }
GuillaumeCH 0:38dbe2988e77 171
GuillaumeCH 0:38dbe2988e77 172 void motors_stop()
GuillaumeCH 0:38dbe2988e77 173 {
GuillaumeCH 0:38dbe2988e77 174 moteurs_arret=1;
GuillaumeCH 0:38dbe2988e77 175 motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
GuillaumeCH 0:38dbe2988e77 176 motors[1]->prepare_hard_hiz();
GuillaumeCH 0:38dbe2988e77 177 x_nucleo_ihm02a1->perform_prepared_actions();
GuillaumeCH 0:38dbe2988e77 178 }
GuillaumeCH 0:38dbe2988e77 179
GuillaumeCH 0:38dbe2988e77 180 void motors_on()
GuillaumeCH 0:38dbe2988e77 181 {
GuillaumeCH 0:38dbe2988e77 182 moteurs_arret=0;
GuillaumeCH 0:38dbe2988e77 183 }
GuillaumeCH 0:38dbe2988e77 184
GuillaumeCH 0:38dbe2988e77 185
GuillaumeCH 0:38dbe2988e77 186 void allumer_del()
GuillaumeCH 0:38dbe2988e77 187 {
GuillaumeCH 0:38dbe2988e77 188 led = 1;
GuillaumeCH 0:38dbe2988e77 189 }
GuillaumeCH 0:38dbe2988e77 190
GuillaumeCH 0:38dbe2988e77 191 void eteindre_del()
GuillaumeCH 0:38dbe2988e77 192 {
GuillaumeCH 0:38dbe2988e77 193 led = 0;
GuillaumeCH 0:38dbe2988e77 194 }
GuillaumeCH 0:38dbe2988e77 195
GuillaumeCH 0:38dbe2988e77 196 void delay_ms()
GuillaumeCH 0:38dbe2988e77 197 {
GuillaumeCH 0:38dbe2988e77 198 }
GuillaumeCH 0:38dbe2988e77 199
GuillaumeCH 0:38dbe2988e77 200 void allumer_autres_del()
GuillaumeCH 0:38dbe2988e77 201 {
GuillaumeCH 0:38dbe2988e77 202 }
GuillaumeCH 0:38dbe2988e77 203
GuillaumeCH 0:38dbe2988e77 204 void eteindre_autres_del()
GuillaumeCH 0:38dbe2988e77 205 {
GuillaumeCH 0:38dbe2988e77 206 }
GuillaumeCH 0:38dbe2988e77 207 void toggle_autres_del() {}
GuillaumeCH 0:38dbe2988e77 208
GuillaumeCH 0:38dbe2988e77 209 void set_all_led()
GuillaumeCH 0:38dbe2988e77 210 {
GuillaumeCH 0:38dbe2988e77 211
GuillaumeCH 0:38dbe2988e77 212 }
GuillaumeCH 0:38dbe2988e77 213
GuillaumeCH 0:38dbe2988e77 214
GuillaumeCH 2:3066e614372f 215 volatile int lastEncodedA = 0;
GuillaumeCH 2:3066e614372f 216 long lastencoderValueA = 0;
GuillaumeCH 2:3066e614372f 217 int lastMSBA = 0;
GuillaumeCH 2:3066e614372f 218 int lastLSBA = 0;
GuillaumeCH 2:3066e614372f 219
GuillaumeCH 2:3066e614372f 220 void updateEncoderA()
GuillaumeCH 2:3066e614372f 221 {
GuillaumeCH 2:3066e614372f 222 int MSBA = ENCAL.read(); //MSB = most significant bit
GuillaumeCH 2:3066e614372f 223 int LSBA = ENCAJ.read(); //LSB = least significant bit
GuillaumeCH 2:3066e614372f 224
GuillaumeCH 2:3066e614372f 225 int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
GuillaumeCH 2:3066e614372f 226 int sumA = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value
GuillaumeCH 2:3066e614372f 227
GuillaumeCH 2:3066e614372f 228 if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
GuillaumeCH 2:3066e614372f 229 if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;
GuillaumeCH 2:3066e614372f 230
GuillaumeCH 2:3066e614372f 231 lastEncodedA = encodedA; //store this value for next time
GuillaumeCH 2:3066e614372f 232 }
GuillaumeCH 2:3066e614372f 233
GuillaumeCH 2:3066e614372f 234
GuillaumeCH 2:3066e614372f 235 volatile int lastEncodedB = 0;
GuillaumeCH 2:3066e614372f 236 long lastencoderValueB = 0;
GuillaumeCH 2:3066e614372f 237 int lastMSBB = 0;
GuillaumeCH 2:3066e614372f 238 int lastLSBB = 0;
GuillaumeCH 2:3066e614372f 239
GuillaumeCH 2:3066e614372f 240 void updateEncoderB()
GuillaumeCH 2:3066e614372f 241 {
GuillaumeCH 2:3066e614372f 242 int MSBB = ENCBL.read(); //MSB = most significant bit
GuillaumeCH 2:3066e614372f 243 int LSBB = ENCBJ.read(); //LSB = least significant bit
GuillaumeCH 2:3066e614372f 244
GuillaumeCH 2:3066e614372f 245 int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
GuillaumeCH 2:3066e614372f 246 int sumB = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
GuillaumeCH 2:3066e614372f 247
GuillaumeCH 2:3066e614372f 248 if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
GuillaumeCH 2:3066e614372f 249 if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
GuillaumeCH 2:3066e614372f 250
GuillaumeCH 2:3066e614372f 251 lastEncodedB = encodedB; //store this value for next time
GuillaumeCH 2:3066e614372f 252 }
GuillaumeCH 2:3066e614372f 253
GuillaumeCH 2:3066e614372f 254
GuillaumeCH 2:3066e614372f 255
GuillaumeCH 2:3066e614372f 256 void debugEncoder()
GuillaumeCH 2:3066e614372f 257 {
GuillaumeCH 2:3066e614372f 258 printf("tick_D : %ld, tick_G : %ld\n", get_nbr_tick_D(),get_nbr_tick_G());
GuillaumeCH 2:3066e614372f 259 }
GuillaumeCH 2:3066e614372f 260 long int get_position_G(){
GuillaumeCH 2:3066e614372f 261 /* Getting the current position. */
GuillaumeCH 2:3066e614372f 262 long int position = motors[1]->get_position();
GuillaumeCH 2:3066e614372f 263 return position;
GuillaumeCH 2:3066e614372f 264 /* Printing to the console. */
GuillaumeCH 2:3066e614372f 265 //printf("--> Getting the current position: %d\r\n", position);
GuillaumeCH 2:3066e614372f 266
GuillaumeCH 2:3066e614372f 267 }
GuillaumeCH 2:3066e614372f 268 long int get_position_D(){
GuillaumeCH 2:3066e614372f 269 /* Getting the current position. */
GuillaumeCH 2:3066e614372f 270 long int position = motors[0]->get_position();
GuillaumeCH 2:3066e614372f 271
GuillaumeCH 2:3066e614372f 272 /* Printing to the console. */
GuillaumeCH 2:3066e614372f 273 //printf("--> Getting the current position: %d\r\n", position);
GuillaumeCH 2:3066e614372f 274 return position;
GuillaumeCH 2:3066e614372f 275
GuillaumeCH 2:3066e614372f 276 }
GuillaumeCH 3:d38aa400d5e7 277
GuillaumeCH 3:d38aa400d5e7 278 void bouton(){
GuillaumeCH 3:d38aa400d5e7 279 DigitalIn depart(USER_BUTTON);
GuillaumeCH 4:eac6746544fb 280 while (depart){
GuillaumeCH 4:eac6746544fb 281 //printf("attente\n");
GuillaumeCH 4:eac6746544fb 282 }
GuillaumeCH 3:d38aa400d5e7 283 }