l

Dependencies:   mbed Asser2

Committer:
JimmyAREM
Date:
Sun May 26 14:57:54 2019 +0000
Revision:
7:6b15a1feed2d
Parent:
5:3638d7e7c5c1
a

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]. */
JimmyAREM 7:6b15a1feed2d 27 4.06, /* Holding kval [V]. */
JimmyAREM 7:6b15a1feed2d 28 4.06, /* Constant speed kval [V]. */
JimmyAREM 7:6b15a1feed2d 29 4.06, /* Acceleration starting kval [V]. */
JimmyAREM 7:6b15a1feed2d 30 4.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]. */
JimmyAREM 7:6b15a1feed2d 55 4.06, /* Holding kval [V]. */
JimmyAREM 7:6b15a1feed2d 56 4.06, /* Constant speed kval [V]. */
JimmyAREM 7:6b15a1feed2d 57 4.06, /* Acceleration starting kval [V]. */
JimmyAREM 7:6b15a1feed2d 58 4.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);
JimmyAREM 7:6b15a1feed2d 112
JimmyAREM 7:6b15a1feed2d 113 encoderValueA = 0;
JimmyAREM 7:6b15a1feed2d 114 encoderValueB = 0;
GuillaumeCH 0:38dbe2988e77 115
GuillaumeCH 0:38dbe2988e77 116 }
GuillaumeCH 0:38dbe2988e77 117
GuillaumeCH 0:38dbe2988e77 118 void set_PWM_moteur_D(int PWM)
GuillaumeCH 0:38dbe2988e77 119 {
GuillaumeCH 0:38dbe2988e77 120 if (!moteurs_arret) {
GuillaumeCH 0:38dbe2988e77 121 if (PWM > PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 122 motors[0]->prepare_run(StepperMotor::BWD, PWM_MAX); //BWD = backward , FWD = forward , la vitesse doit etre positive
GuillaumeCH 0:38dbe2988e77 123 } else if (PWM <-PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 124 motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
GuillaumeCH 0:38dbe2988e77 125 } else if (PWM > 0) {
GuillaumeCH 0:38dbe2988e77 126 motors[0]->prepare_run(StepperMotor::BWD, PWM);
GuillaumeCH 0:38dbe2988e77 127 } else if (PWM < 0) {
GuillaumeCH 0:38dbe2988e77 128 motors[0]->prepare_run(StepperMotor::FWD, -PWM);
GuillaumeCH 0:38dbe2988e77 129 } else if (PWM == 0) {
GuillaumeCH 0:38dbe2988e77 130 motors[0]->prepare_run(StepperMotor::BWD, 0);
GuillaumeCH 0:38dbe2988e77 131 }
GuillaumeCH 0:38dbe2988e77 132 } else {
GuillaumeCH 0:38dbe2988e77 133 motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
GuillaumeCH 0:38dbe2988e77 134 }
GuillaumeCH 0:38dbe2988e77 135 x_nucleo_ihm02a1->perform_prepared_actions();
GuillaumeCH 0:38dbe2988e77 136 }
GuillaumeCH 0:38dbe2988e77 137
GuillaumeCH 0:38dbe2988e77 138 void set_PWM_moteur_G(int PWM)
GuillaumeCH 0:38dbe2988e77 139 {
GuillaumeCH 0:38dbe2988e77 140
GuillaumeCH 0:38dbe2988e77 141 if (!moteurs_arret) {
GuillaumeCH 0:38dbe2988e77 142 if (PWM > PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 143 motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
GuillaumeCH 0:38dbe2988e77 144 } else if (PWM <-PWM_MAX) {
GuillaumeCH 0:38dbe2988e77 145 motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
GuillaumeCH 0:38dbe2988e77 146 } else if (PWM > 0) {
GuillaumeCH 0:38dbe2988e77 147 motors[1]->prepare_run(StepperMotor::FWD, PWM);
GuillaumeCH 0:38dbe2988e77 148 } else if (PWM < 0) {
GuillaumeCH 0:38dbe2988e77 149 motors[1]->prepare_run(StepperMotor::BWD, -PWM);
GuillaumeCH 0:38dbe2988e77 150 } else if (PWM == 0) {
GuillaumeCH 0:38dbe2988e77 151 motors[1]->prepare_run(StepperMotor::BWD, 0);
GuillaumeCH 0:38dbe2988e77 152 }
GuillaumeCH 0:38dbe2988e77 153 } else {
GuillaumeCH 0:38dbe2988e77 154 motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
GuillaumeCH 0:38dbe2988e77 155 }
GuillaumeCH 0:38dbe2988e77 156 x_nucleo_ihm02a1->perform_prepared_actions();
GuillaumeCH 0:38dbe2988e77 157 }
GuillaumeCH 0:38dbe2988e77 158
GuillaumeCH 2:3066e614372f 159
GuillaumeCH 2:3066e614372f 160 long int get_nbr_tick_D()
GuillaumeCH 2:3066e614372f 161 {
GuillaumeCH 2:3066e614372f 162 return encoderValueA;
GuillaumeCH 0:38dbe2988e77 163 }
GuillaumeCH 2:3066e614372f 164
GuillaumeCH 0:38dbe2988e77 165 long int get_nbr_tick_G()
GuillaumeCH 0:38dbe2988e77 166 {
GuillaumeCH 0:38dbe2988e77 167 return encoderValueB;
GuillaumeCH 0:38dbe2988e77 168 }
GuillaumeCH 0:38dbe2988e77 169
GuillaumeCH 0:38dbe2988e77 170 void attente_synchro()
GuillaumeCH 0:38dbe2988e77 171 {
GuillaumeCH 0:38dbe2988e77 172 //structute du temps d'attente de l'asservissement 10ms
GuillaumeCH 0:38dbe2988e77 173 wait(0.010);
GuillaumeCH 0:38dbe2988e77 174 }
GuillaumeCH 0:38dbe2988e77 175
GuillaumeCH 0:38dbe2988e77 176 void motors_stop()
GuillaumeCH 0:38dbe2988e77 177 {
GuillaumeCH 0:38dbe2988e77 178 moteurs_arret=1;
GuillaumeCH 0:38dbe2988e77 179 motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
GuillaumeCH 0:38dbe2988e77 180 motors[1]->prepare_hard_hiz();
GuillaumeCH 0:38dbe2988e77 181 x_nucleo_ihm02a1->perform_prepared_actions();
GuillaumeCH 0:38dbe2988e77 182 }
GuillaumeCH 0:38dbe2988e77 183
GuillaumeCH 0:38dbe2988e77 184 void motors_on()
GuillaumeCH 0:38dbe2988e77 185 {
GuillaumeCH 0:38dbe2988e77 186 moteurs_arret=0;
GuillaumeCH 0:38dbe2988e77 187 }
GuillaumeCH 0:38dbe2988e77 188
GuillaumeCH 0:38dbe2988e77 189
GuillaumeCH 0:38dbe2988e77 190 void allumer_del()
GuillaumeCH 0:38dbe2988e77 191 {
GuillaumeCH 0:38dbe2988e77 192 led = 1;
GuillaumeCH 0:38dbe2988e77 193 }
GuillaumeCH 0:38dbe2988e77 194
GuillaumeCH 0:38dbe2988e77 195 void eteindre_del()
GuillaumeCH 0:38dbe2988e77 196 {
GuillaumeCH 0:38dbe2988e77 197 led = 0;
GuillaumeCH 0:38dbe2988e77 198 }
GuillaumeCH 0:38dbe2988e77 199
GuillaumeCH 0:38dbe2988e77 200 void delay_ms()
GuillaumeCH 0:38dbe2988e77 201 {
GuillaumeCH 0:38dbe2988e77 202 }
GuillaumeCH 0:38dbe2988e77 203
GuillaumeCH 0:38dbe2988e77 204 void allumer_autres_del()
GuillaumeCH 0:38dbe2988e77 205 {
GuillaumeCH 0:38dbe2988e77 206 }
GuillaumeCH 0:38dbe2988e77 207
GuillaumeCH 0:38dbe2988e77 208 void eteindre_autres_del()
GuillaumeCH 0:38dbe2988e77 209 {
GuillaumeCH 0:38dbe2988e77 210 }
GuillaumeCH 0:38dbe2988e77 211 void toggle_autres_del() {}
GuillaumeCH 0:38dbe2988e77 212
GuillaumeCH 0:38dbe2988e77 213 void set_all_led()
GuillaumeCH 0:38dbe2988e77 214 {
GuillaumeCH 0:38dbe2988e77 215
GuillaumeCH 0:38dbe2988e77 216 }
GuillaumeCH 0:38dbe2988e77 217
GuillaumeCH 0:38dbe2988e77 218
GuillaumeCH 2:3066e614372f 219 volatile int lastEncodedA = 0;
GuillaumeCH 2:3066e614372f 220 long lastencoderValueA = 0;
GuillaumeCH 2:3066e614372f 221 int lastMSBA = 0;
GuillaumeCH 2:3066e614372f 222 int lastLSBA = 0;
GuillaumeCH 2:3066e614372f 223
GuillaumeCH 2:3066e614372f 224 void updateEncoderA()
GuillaumeCH 2:3066e614372f 225 {
GuillaumeCH 2:3066e614372f 226 int MSBA = ENCAL.read(); //MSB = most significant bit
GuillaumeCH 2:3066e614372f 227 int LSBA = ENCAJ.read(); //LSB = least significant bit
GuillaumeCH 2:3066e614372f 228
GuillaumeCH 2:3066e614372f 229 int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
GuillaumeCH 2:3066e614372f 230 int sumA = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value
GuillaumeCH 2:3066e614372f 231
GuillaumeCH 2:3066e614372f 232 if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
GuillaumeCH 2:3066e614372f 233 if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;
GuillaumeCH 2:3066e614372f 234
GuillaumeCH 2:3066e614372f 235 lastEncodedA = encodedA; //store this value for next time
GuillaumeCH 2:3066e614372f 236 }
GuillaumeCH 2:3066e614372f 237
GuillaumeCH 2:3066e614372f 238
GuillaumeCH 2:3066e614372f 239 volatile int lastEncodedB = 0;
GuillaumeCH 2:3066e614372f 240 long lastencoderValueB = 0;
GuillaumeCH 2:3066e614372f 241 int lastMSBB = 0;
GuillaumeCH 2:3066e614372f 242 int lastLSBB = 0;
GuillaumeCH 2:3066e614372f 243
GuillaumeCH 2:3066e614372f 244 void updateEncoderB()
GuillaumeCH 2:3066e614372f 245 {
GuillaumeCH 2:3066e614372f 246 int MSBB = ENCBL.read(); //MSB = most significant bit
GuillaumeCH 2:3066e614372f 247 int LSBB = ENCBJ.read(); //LSB = least significant bit
GuillaumeCH 2:3066e614372f 248
GuillaumeCH 2:3066e614372f 249 int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
GuillaumeCH 2:3066e614372f 250 int sumB = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
GuillaumeCH 2:3066e614372f 251
GuillaumeCH 2:3066e614372f 252 if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
GuillaumeCH 2:3066e614372f 253 if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
GuillaumeCH 2:3066e614372f 254
GuillaumeCH 2:3066e614372f 255 lastEncodedB = encodedB; //store this value for next time
GuillaumeCH 2:3066e614372f 256 }
GuillaumeCH 2:3066e614372f 257
GuillaumeCH 2:3066e614372f 258
GuillaumeCH 2:3066e614372f 259
GuillaumeCH 2:3066e614372f 260 void debugEncoder()
GuillaumeCH 2:3066e614372f 261 {
GuillaumeCH 2:3066e614372f 262 printf("tick_D : %ld, tick_G : %ld\n", get_nbr_tick_D(),get_nbr_tick_G());
GuillaumeCH 2:3066e614372f 263 }
GuillaumeCH 2:3066e614372f 264 long int get_position_G(){
GuillaumeCH 2:3066e614372f 265 /* Getting the current position. */
GuillaumeCH 2:3066e614372f 266 long int position = motors[1]->get_position();
GuillaumeCH 2:3066e614372f 267 return position;
GuillaumeCH 2:3066e614372f 268 /* Printing to the console. */
GuillaumeCH 2:3066e614372f 269 //printf("--> Getting the current position: %d\r\n", position);
GuillaumeCH 2:3066e614372f 270
GuillaumeCH 2:3066e614372f 271 }
GuillaumeCH 2:3066e614372f 272 long int get_position_D(){
GuillaumeCH 2:3066e614372f 273 /* Getting the current position. */
GuillaumeCH 2:3066e614372f 274 long int position = motors[0]->get_position();
GuillaumeCH 2:3066e614372f 275
GuillaumeCH 2:3066e614372f 276 /* Printing to the console. */
GuillaumeCH 2:3066e614372f 277 //printf("--> Getting the current position: %d\r\n", position);
GuillaumeCH 2:3066e614372f 278 return position;
GuillaumeCH 2:3066e614372f 279
GuillaumeCH 2:3066e614372f 280 }
GuillaumeCH 3:d38aa400d5e7 281
GuillaumeCH 3:d38aa400d5e7 282 void bouton(){
GuillaumeCH 3:d38aa400d5e7 283 DigitalIn depart(USER_BUTTON);
GuillaumeCH 4:eac6746544fb 284 while (depart){
GuillaumeCH 4:eac6746544fb 285 //printf("attente\n");
GuillaumeCH 4:eac6746544fb 286 }
GuillaumeCH 3:d38aa400d5e7 287 }