Corentin Courtot / Mbed 2 deprecated Asserve

asser

Dependencies:   mbed X_NUCLEO_IHM02A1

hardware.cpp@0:6ca63d45f0ee, 2018-12-11 (annotated)

Committer:
Coconara
Date:
Tue Dec 11 19:12:55 2018 +0000
Revision:
0:6ca63d45f0ee
Child:
2:5764f89a27f6
asser

Who changed what in which revision?

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