Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

hardware.cpp@5:3638d7e7c5c1, 2019-05-20 (annotated)

Committer:
JimmyAREM
Date:
Mon May 20 23:08:09 2019 +0000
Revision:
5:3638d7e7c5c1
Parent:
4:eac6746544fb
Child:
7:6b15a1feed2d
maj pour publish

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