Concours voiture autonome / Mbed 2 deprecated MOTEUR_LPC_TEST_V2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed PROGRAMME_MOTEUR_V1

main.cpp@2:ce23bd78a43a, 2021-04-15 (annotated)

Committer:
QuentinD
Date:
Thu Apr 15 14:53:13 2021 +0000
Revision:
2:ce23bd78a43a
Parent:
1:b71ac293907d 
programme LPC avec ajustement de la direction pour le voiture A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jeanmt 0:a0ab83db7fa0 1 // Display changes in encoder position and direction
jeanmt 0:a0ab83db7fa0 2 #include "mbed.h"
jeanmt 0:a0ab83db7fa0 3 #include "qeihw.h"
QuentinD 2:ce23bd78a43a 4 #define PLAGE_ANGULAIRE 800 // 800 // 420
QuentinD 2:ce23bd78a43a 5 #define GAUCHE_MAX 500 // 500 /// 720
QuentinD 2:ce23bd78a43a 6 #define TOUT_DROIT 900
QuentinD 2:ce23bd78a43a 7 #define ARRET_MOTEUR_NON_VOULU 3
jeanmt 0:a0ab83db7fa0 8 #define BLINKING_RATE_MS
jeanmt 1:b71ac293907d 9 #define CAPTEUR_TEMPO 200
jeanmt 1:b71ac293907d 10 #define CAN_TEMPO 50
jeanmt 1:b71ac293907d 11 #define MOTEUR_TEMPO 50
jeanmt 0:a0ab83db7fa0 12 #include "mbed.h"
jeanmt 1:b71ac293907d 13 #define POS_PER_METER 43636
QuentinD 2:ce23bd78a43a 14 //#define GAUCHE_MAX 710
QuentinD 2:ce23bd78a43a 15 //#define PLAGE_ANGULAIRE 420
jeanmt 0:a0ab83db7fa0 16
jeanmt 0:a0ab83db7fa0 17 CAN can2(p30, p29,1000000);
jeanmt 0:a0ab83db7fa0 18
jeanmt 0:a0ab83db7fa0 19 PwmOut Servo(p23);
jeanmt 0:a0ab83db7fa0 20 PwmOut pwm_mot(p26);
jeanmt 1:b71ac293907d 21 Serial usb(USBTX,USBRX,115200) ;
jeanmt 0:a0ab83db7fa0 22 DigitalOut inA(p24);
jeanmt 0:a0ab83db7fa0 23 DigitalOut inB(p25);
jeanmt 0:a0ab83db7fa0 24 DigitalOut led1(LED1);
jeanmt 0:a0ab83db7fa0 25 DigitalOut led3(LED3);
jeanmt 0:a0ab83db7fa0 26
QuentinD 2:ce23bd78a43a 27 void print (void);
QuentinD 2:ce23bd78a43a 28
QuentinD 2:ce23bd78a43a 29 void serial_ISR(void);
QuentinD 2:ce23bd78a43a 30
jeanmt 1:b71ac293907d 31 int Bytes2Int(char data[], int position ) ;
jeanmt 1:b71ac293907d 32
jeanmt 1:b71ac293907d 33 void set_position(unsigned short us) ;
jeanmt 0:a0ab83db7fa0 34
jeanmt 1:b71ac293907d 35 void send_can (float vitesse, int nbr) ;
jeanmt 0:a0ab83db7fa0 36
QuentinD 2:ce23bd78a43a 37 void send_can2(bool Moteur_stopper);
QuentinD 2:ce23bd78a43a 38
jeanmt 1:b71ac293907d 39 float Bytes2Float( char data[], int position) ;
jeanmt 1:b71ac293907d 40
jeanmt 1:b71ac293907d 41 float Float2Bytes( char data[], float nbr) ;
jeanmt 0:a0ab83db7fa0 42
jeanmt 0:a0ab83db7fa0 43 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
jeanmt 0:a0ab83db7fa0 44 Timer tempo ;
jeanmt 0:a0ab83db7fa0 45 Timer can_timer ;
jeanmt 1:b71ac293907d 46 Timer mot_timer ;
jeanmt 1:b71ac293907d 47 Timer capteur_timer ;
QuentinD 2:ce23bd78a43a 48 Timer Probleme_arret;
QuentinD 2:ce23bd78a43a 49
QuentinD 2:ce23bd78a43a 50 unsigned short pos = 1000;
QuentinD 2:ce23bd78a43a 51 float vit = 0 ;
QuentinD 2:ce23bd78a43a 52
jeanmt 0:a0ab83db7fa0 53 int main() {
jeanmt 0:a0ab83db7fa0 54
QuentinD 2:ce23bd78a43a 55 int32_t Position2, position = 0 , temp = 0, premier_probleme = 0;
jeanmt 1:b71ac293907d 56 float vitesse = 1.05 , direction , ValMot, ValServo ;
jeanmt 0:a0ab83db7fa0 57 unsigned char *chptr;
QuentinD 2:ce23bd78a43a 58 char Sens = 1, Moteur_stopper = 0;
QuentinD 2:ce23bd78a43a 59 int ajustement_direction = 0;
jeanmt 1:b71ac293907d 60
jeanmt 0:a0ab83db7fa0 61 CANMessage msg_rx;
jeanmt 0:a0ab83db7fa0 62 CANMessage msg_tx;
jeanmt 0:a0ab83db7fa0 63
QuentinD 2:ce23bd78a43a 64 usb.attach(&serial_ISR,Serial::RxIrq);
jeanmt 0:a0ab83db7fa0 65
jeanmt 1:b71ac293907d 66
jeanmt 0:a0ab83db7fa0 67 qei.SetDigiFilter(10);
jeanmt 0:a0ab83db7fa0 68 qei.SetMaxPosition(0xFFFFFFFF);
jeanmt 1:b71ac293907d 69
jeanmt 1:b71ac293907d 70 mot_timer.start() ;
jeanmt 1:b71ac293907d 71 can_timer.start() ;
QuentinD 2:ce23bd78a43a 72 Probleme_arret.start();
jeanmt 1:b71ac293907d 73 capteur_timer.start() ;
jeanmt 1:b71ac293907d 74 tempo.start() ;
jeanmt 1:b71ac293907d 75
jeanmt 0:a0ab83db7fa0 76
jeanmt 0:a0ab83db7fa0 77 Servo.period_ms(20);
QuentinD 2:ce23bd78a43a 78 pwm_mot.period_us(50);
QuentinD 2:ce23bd78a43a 79 inA = 1;
QuentinD 2:ce23bd78a43a 80 inB = 0;
jeanmt 0:a0ab83db7fa0 81 while(1)
jeanmt 1:b71ac293907d 82 {
jeanmt 0:a0ab83db7fa0 83 if (can2.read(msg_rx))
jeanmt 0:a0ab83db7fa0 84 {
QuentinD 2:ce23bd78a43a 85 ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;
QuentinD 2:ce23bd78a43a 86 if(ValServo>=TOUT_DROIT)
QuentinD 2:ce23bd78a43a 87 {
QuentinD 2:ce23bd78a43a 88 ajustement_direction = (ValServo - TOUT_DROIT)*0.33;
QuentinD 2:ce23bd78a43a 89 ValServo = ValServo - ajustement_direction;
QuentinD 2:ce23bd78a43a 90 }
QuentinD 2:ce23bd78a43a 91
jeanmt 0:a0ab83db7fa0 92 ValMot = float(msg_rx.data[1]/255.0f);
jeanmt 0:a0ab83db7fa0 93 Sens = msg_rx.data[2] ;
jeanmt 0:a0ab83db7fa0 94 inA = Sens;
jeanmt 0:a0ab83db7fa0 95 inB = !Sens;
jeanmt 0:a0ab83db7fa0 96
jeanmt 0:a0ab83db7fa0 97 }
jeanmt 0:a0ab83db7fa0 98
jeanmt 1:b71ac293907d 99 if (capteur_timer.read_ms() > CAPTEUR_TEMPO){
jeanmt 0:a0ab83db7fa0 100 Position2 = qei.GetPosition();
jeanmt 1:b71ac293907d 101 led1 = qei.Direction() == SET ? 1 : 0 ;
jeanmt 0:a0ab83db7fa0 102 led3 = !led1;
jeanmt 1:b71ac293907d 103 vitesse = float(Position2 - position)/(POS_PER_METER*0.020) ;
jeanmt 1:b71ac293907d 104 //usb.printf("la pwm est de %f la position est de %lu la vitesse est de %f km/h\r",ValMot, Position2,vitesse);
jeanmt 0:a0ab83db7fa0 105 position = Position2 ;
jeanmt 1:b71ac293907d 106 capteur_timer.reset() ;
jeanmt 0:a0ab83db7fa0 107 }
jeanmt 1:b71ac293907d 108
jeanmt 1:b71ac293907d 109
jeanmt 0:a0ab83db7fa0 110 set_position(ValServo);
jeanmt 0:a0ab83db7fa0 111 pwm_mot.write(ValMot);
QuentinD 2:ce23bd78a43a 112 //set_position(pos);
QuentinD 2:ce23bd78a43a 113 //pwm_mot.write(vit);
jeanmt 1:b71ac293907d 114
jeanmt 1:b71ac293907d 115
jeanmt 1:b71ac293907d 116
jeanmt 1:b71ac293907d 117
QuentinD 2:ce23bd78a43a 118 if(can_timer.read_ms() > CAN_TEMPO )
QuentinD 2:ce23bd78a43a 119 {
QuentinD 2:ce23bd78a43a 120 if (vitesse <= ARRET_MOTEUR_NON_VOULU)
QuentinD 2:ce23bd78a43a 121 {
QuentinD 2:ce23bd78a43a 122 if(premier_probleme == 0)
QuentinD 2:ce23bd78a43a 123 premier_probleme = Probleme_arret.read_ms();
QuentinD 2:ce23bd78a43a 124 if(Probleme_arret.read_ms() >= premier_probleme + 5000);
QuentinD 2:ce23bd78a43a 125 Moteur_stopper = 1;
QuentinD 2:ce23bd78a43a 126 if(vitesse >= ARRET_MOTEUR_NON_VOULU)
QuentinD 2:ce23bd78a43a 127 {
QuentinD 2:ce23bd78a43a 128 Moteur_stopper = 0;
QuentinD 2:ce23bd78a43a 129 premier_probleme = 0;
QuentinD 2:ce23bd78a43a 130 }
QuentinD 2:ce23bd78a43a 131 }
jeanmt 1:b71ac293907d 132 send_can(vitesse, position) ;
QuentinD 2:ce23bd78a43a 133 send_can2(Moteur_stopper);
jeanmt 1:b71ac293907d 134 can_timer.reset() ;
QuentinD 2:ce23bd78a43a 135 }
jeanmt 0:a0ab83db7fa0 136
jeanmt 0:a0ab83db7fa0 137
jeanmt 0:a0ab83db7fa0 138
jeanmt 1:b71ac293907d 139 }
jeanmt 0:a0ab83db7fa0 140 }
jeanmt 0:a0ab83db7fa0 141
jeanmt 0:a0ab83db7fa0 142 /******* Régulation PID ********/
jeanmt 0:a0ab83db7fa0 143 // Ecart entre la consigne et la mesure
jeanmt 0:a0ab83db7fa0 144 // ecart = vref - vitesse;
jeanmt 0:a0ab83db7fa0 145
jeanmt 0:a0ab83db7fa0 146 // Terme proportionnel
jeanmt 0:a0ab83db7fa0 147 // P_x = Kp * ecart;
jeanmt 0:a0ab83db7fa0 148
jeanmt 0:a0ab83db7fa0 149 // Calcul de la commande
jeanmt 0:a0ab83db7fa0 150 // commande = P_x + I_x;
jeanmt 0:a0ab83db7fa0 151
jeanmt 0:a0ab83db7fa0 152 // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
jeanmt 0:a0ab83db7fa0 153 // I_x = I_x + Ki * dt * ecart;
jeanmt 0:a0ab83db7fa0 154 /******* Fin régulation PID ********/
jeanmt 0:a0ab83db7fa0 155
jeanmt 0:a0ab83db7fa0 156
jeanmt 0:a0ab83db7fa0 157
jeanmt 0:a0ab83db7fa0 158
jeanmt 1:b71ac293907d 159 void send_can (float vitesse, int nbr){
jeanmt 1:b71ac293907d 160 float res ;
jeanmt 1:b71ac293907d 161 int res2 ;
jeanmt 1:b71ac293907d 162 unsigned char *chptr ;
jeanmt 1:b71ac293907d 163 CANMessage message;
jeanmt 1:b71ac293907d 164 message.len=8;
jeanmt 1:b71ac293907d 165 chptr = (unsigned char *) & vitesse;
jeanmt 1:b71ac293907d 166 message.data[0] = (*chptr++);
jeanmt 1:b71ac293907d 167 message.data[1] = (*chptr++);
jeanmt 1:b71ac293907d 168 message.data[2] = (*chptr++);
jeanmt 1:b71ac293907d 169 message.data[3] = (*chptr);
jeanmt 1:b71ac293907d 170 message.data[4] = char(nbr) ;
jeanmt 1:b71ac293907d 171 message.data[5] = char(nbr>>8) ;
jeanmt 1:b71ac293907d 172 message.data[6] = char(nbr>>16) ;
jeanmt 1:b71ac293907d 173 message.data[7] = char(nbr>>24) ;
QuentinD 2:ce23bd78a43a 174
jeanmt 1:b71ac293907d 175 message.id=0x400;
jeanmt 1:b71ac293907d 176
jeanmt 1:b71ac293907d 177 res = Bytes2Float((char*)message.data, 0) ;
jeanmt 1:b71ac293907d 178 res2 = Bytes2Int((char*)message.data, 4) ;
jeanmt 1:b71ac293907d 179 usb.printf("\rvitesse = %f, pos = %d", vitesse, nbr) ;
jeanmt 1:b71ac293907d 180 message.type=CANData;
jeanmt 1:b71ac293907d 181 message.format=CANStandard;
jeanmt 1:b71ac293907d 182 can2.write(message);
jeanmt 1:b71ac293907d 183
jeanmt 1:b71ac293907d 184 }
QuentinD 2:ce23bd78a43a 185 void send_can2(bool Moteur_stopper)
QuentinD 2:ce23bd78a43a 186 {
QuentinD 2:ce23bd78a43a 187 CANMessage message2;
QuentinD 2:ce23bd78a43a 188 message2.len=1;
QuentinD 2:ce23bd78a43a 189 message2.data[0] = bool(Moteur_stopper);
QuentinD 2:ce23bd78a43a 190
QuentinD 2:ce23bd78a43a 191 message2.id=0x500;
QuentinD 2:ce23bd78a43a 192
QuentinD 2:ce23bd78a43a 193 usb.printf("\rARRET MOTEUR %d",Moteur_stopper);
QuentinD 2:ce23bd78a43a 194
QuentinD 2:ce23bd78a43a 195 message2.type=CANData;
QuentinD 2:ce23bd78a43a 196 message2.format=CANStandard;
QuentinD 2:ce23bd78a43a 197 can2.write(message2);
QuentinD 2:ce23bd78a43a 198
QuentinD 2:ce23bd78a43a 199 }
jeanmt 0:a0ab83db7fa0 200
jeanmt 0:a0ab83db7fa0 201
jeanmt 0:a0ab83db7fa0 202 void set_position(unsigned short us)
jeanmt 0:a0ab83db7fa0 203 {
jeanmt 0:a0ab83db7fa0 204 unsigned short period= us+20000;
jeanmt 0:a0ab83db7fa0 205 float pulse_width;
QuentinD 2:ce23bd78a43a 206 if (tempo.read_ms() > 50)
jeanmt 0:a0ab83db7fa0 207 {
jeanmt 0:a0ab83db7fa0 208 pulse_width=((float)us)/((float)period);
jeanmt 0:a0ab83db7fa0 209 Servo.period_us(period);
jeanmt 0:a0ab83db7fa0 210 Servo.write(pulse_width);
jeanmt 0:a0ab83db7fa0 211 tempo.reset();
jeanmt 0:a0ab83db7fa0 212 }
jeanmt 0:a0ab83db7fa0 213 }
jeanmt 1:b71ac293907d 214
jeanmt 1:b71ac293907d 215 int Bytes2Int(char data[], int position ){
jeanmt 1:b71ac293907d 216 int nbr = 0 ;
jeanmt 1:b71ac293907d 217 return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
jeanmt 1:b71ac293907d 218 }
jeanmt 1:b71ac293907d 219
jeanmt 1:b71ac293907d 220
jeanmt 1:b71ac293907d 221 float Float2Bytes( char data[], float nbr){
jeanmt 1:b71ac293907d 222 unsigned char* chptr ;
jeanmt 1:b71ac293907d 223 chptr = (unsigned char *) & nbr;
jeanmt 1:b71ac293907d 224 data[0]= (*chptr++);
jeanmt 1:b71ac293907d 225 data[1]= (*chptr++);
jeanmt 1:b71ac293907d 226 data[2]= (*chptr++);
jeanmt 1:b71ac293907d 227 data[3]= (*chptr);
jeanmt 1:b71ac293907d 228
jeanmt 1:b71ac293907d 229 }
jeanmt 1:b71ac293907d 230
jeanmt 1:b71ac293907d 231 float Bytes2Float( char data[], int position){
jeanmt 1:b71ac293907d 232 union {
jeanmt 1:b71ac293907d 233 float float_sent;
jeanmt 1:b71ac293907d 234 unsigned char bytes[4];
jeanmt 1:b71ac293907d 235 } u;
jeanmt 1:b71ac293907d 236 memcpy(u.bytes, data + position,4) ;
jeanmt 1:b71ac293907d 237 return u.float_sent ;
jeanmt 1:b71ac293907d 238 }
QuentinD 2:ce23bd78a43a 239
QuentinD 2:ce23bd78a43a 240 void serial_ISR(void){
QuentinD 2:ce23bd78a43a 241 char msg=usb.getc();
QuentinD 2:ce23bd78a43a 242 switch (msg){
QuentinD 2:ce23bd78a43a 243 case 'a':
QuentinD 2:ce23bd78a43a 244 pos=pos+100;
QuentinD 2:ce23bd78a43a 245 break;
QuentinD 2:ce23bd78a43a 246 case 'z':
QuentinD 2:ce23bd78a43a 247 if (pos>100)
QuentinD 2:ce23bd78a43a 248 pos=pos-100;
QuentinD 2:ce23bd78a43a 249 break;
QuentinD 2:ce23bd78a43a 250 case 'e':
QuentinD 2:ce23bd78a43a 251 pos=pos+10;
QuentinD 2:ce23bd78a43a 252 break;
QuentinD 2:ce23bd78a43a 253 case 'r':
QuentinD 2:ce23bd78a43a 254 if (pos>10)
QuentinD 2:ce23bd78a43a 255 pos=pos-10;
QuentinD 2:ce23bd78a43a 256 break;
QuentinD 2:ce23bd78a43a 257 case 'q':
QuentinD 2:ce23bd78a43a 258 if (vit<1.0)
QuentinD 2:ce23bd78a43a 259 vit+=0.1 ;
QuentinD 2:ce23bd78a43a 260 break;
QuentinD 2:ce23bd78a43a 261 case 's':
QuentinD 2:ce23bd78a43a 262 if (vit>0.0)
QuentinD 2:ce23bd78a43a 263 vit-= 0.1 ;
QuentinD 2:ce23bd78a43a 264 break;
QuentinD 2:ce23bd78a43a 265 case 'd':
QuentinD 2:ce23bd78a43a 266 if (vit<1.0)
QuentinD 2:ce23bd78a43a 267 vit+=0.01 ;
QuentinD 2:ce23bd78a43a 268 break;
QuentinD 2:ce23bd78a43a 269 case 'f':
QuentinD 2:ce23bd78a43a 270 if (vit>0.0)
QuentinD 2:ce23bd78a43a 271 vit-=0.01 ;
QuentinD 2:ce23bd78a43a 272 break;
QuentinD 2:ce23bd78a43a 273
QuentinD 2:ce23bd78a43a 274
QuentinD 2:ce23bd78a43a 275 }
QuentinD 2:ce23bd78a43a 276 usb.printf("pos = %d us vit = %f \n",pos,vit);
QuentinD 2:ce23bd78a43a 277 }