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@1:b71ac293907d, 2021-04-08 (annotated)

Committer:
jeanmt
Date:
Thu Apr 08 14:32:30 2021 +0000
Revision:
1:b71ac293907d
Parent:
0:a0ab83db7fa0
Child:
2:ce23bd78a43a
08 avril 2021

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"
jeanmt 0:a0ab83db7fa0 4 #define PLAGE_ANGULAIRE 420
jeanmt 0:a0ab83db7fa0 5 #define GAUCHE_MAX 710
jeanmt 0:a0ab83db7fa0 6 #define BLINKING_RATE_MS
jeanmt 1:b71ac293907d 7 #define CAPTEUR_TEMPO 200
jeanmt 1:b71ac293907d 8 #define CAN_TEMPO 50
jeanmt 1:b71ac293907d 9 #define MOTEUR_TEMPO 50
jeanmt 0:a0ab83db7fa0 10 #include "mbed.h"
jeanmt 1:b71ac293907d 11 #define POS_PER_METER 43636
jeanmt 1:b71ac293907d 12 #define GAUCHE_MAX 710
jeanmt 1:b71ac293907d 13 #define PLAGE_ANGULAIRE 420
jeanmt 0:a0ab83db7fa0 14
jeanmt 0:a0ab83db7fa0 15 CAN can2(p30, p29,1000000);
jeanmt 0:a0ab83db7fa0 16
jeanmt 0:a0ab83db7fa0 17 PwmOut Servo(p23);
jeanmt 0:a0ab83db7fa0 18 PwmOut pwm_mot(p26);
jeanmt 1:b71ac293907d 19 Serial usb(USBTX,USBRX,115200) ;
jeanmt 0:a0ab83db7fa0 20 DigitalOut inA(p24);
jeanmt 0:a0ab83db7fa0 21 DigitalOut inB(p25);
jeanmt 0:a0ab83db7fa0 22 DigitalOut led1(LED1);
jeanmt 0:a0ab83db7fa0 23 DigitalOut led3(LED3);
jeanmt 0:a0ab83db7fa0 24
jeanmt 1:b71ac293907d 25 int Bytes2Int(char data[], int position ) ;
jeanmt 1:b71ac293907d 26
jeanmt 1:b71ac293907d 27 void set_position(unsigned short us) ;
jeanmt 0:a0ab83db7fa0 28
jeanmt 1:b71ac293907d 29 void send_can (float vitesse, int nbr) ;
jeanmt 0:a0ab83db7fa0 30
jeanmt 1:b71ac293907d 31 float Bytes2Float( char data[], int position) ;
jeanmt 1:b71ac293907d 32
jeanmt 1:b71ac293907d 33 float Float2Bytes( char data[], float nbr) ;
jeanmt 0:a0ab83db7fa0 34
jeanmt 0:a0ab83db7fa0 35 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
jeanmt 0:a0ab83db7fa0 36 Timer tempo ;
jeanmt 0:a0ab83db7fa0 37 Timer can_timer ;
jeanmt 1:b71ac293907d 38 Timer mot_timer ;
jeanmt 1:b71ac293907d 39 Timer capteur_timer ;
jeanmt 0:a0ab83db7fa0 40 int main() {
jeanmt 0:a0ab83db7fa0 41
jeanmt 1:b71ac293907d 42 int32_t Position2, position = 0 , temp = 0;
jeanmt 1:b71ac293907d 43 float vitesse = 1.05 , direction , ValMot, ValServo ;
jeanmt 0:a0ab83db7fa0 44 unsigned char *chptr;
jeanmt 1:b71ac293907d 45 char Sens = 1;
jeanmt 1:b71ac293907d 46
jeanmt 0:a0ab83db7fa0 47 CANMessage msg_rx;
jeanmt 0:a0ab83db7fa0 48 CANMessage msg_tx;
jeanmt 0:a0ab83db7fa0 49
jeanmt 1:b71ac293907d 50
jeanmt 0:a0ab83db7fa0 51
jeanmt 1:b71ac293907d 52
jeanmt 0:a0ab83db7fa0 53 qei.SetDigiFilter(10);
jeanmt 0:a0ab83db7fa0 54 qei.SetMaxPosition(0xFFFFFFFF);
jeanmt 1:b71ac293907d 55
jeanmt 1:b71ac293907d 56 mot_timer.start() ;
jeanmt 1:b71ac293907d 57 can_timer.start() ;
jeanmt 1:b71ac293907d 58 capteur_timer.start() ;
jeanmt 1:b71ac293907d 59 tempo.start() ;
jeanmt 1:b71ac293907d 60
jeanmt 0:a0ab83db7fa0 61
jeanmt 0:a0ab83db7fa0 62 Servo.period_ms(20);
jeanmt 0:a0ab83db7fa0 63 pwm_mot.period_ms(20);
jeanmt 0:a0ab83db7fa0 64 while(1)
jeanmt 1:b71ac293907d 65 {
jeanmt 0:a0ab83db7fa0 66 if (can2.read(msg_rx))
jeanmt 0:a0ab83db7fa0 67 {
jeanmt 0:a0ab83db7fa0 68 ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;;
jeanmt 0:a0ab83db7fa0 69 ValMot = float(msg_rx.data[1]/255.0f);
jeanmt 0:a0ab83db7fa0 70 Sens = msg_rx.data[2] ;
jeanmt 0:a0ab83db7fa0 71 inA = Sens;
jeanmt 0:a0ab83db7fa0 72 inB = !Sens;
jeanmt 0:a0ab83db7fa0 73
jeanmt 0:a0ab83db7fa0 74 }
jeanmt 0:a0ab83db7fa0 75
jeanmt 1:b71ac293907d 76 if (capteur_timer.read_ms() > CAPTEUR_TEMPO){
jeanmt 0:a0ab83db7fa0 77 Position2 = qei.GetPosition();
jeanmt 1:b71ac293907d 78 led1 = qei.Direction() == SET ? 1 : 0 ;
jeanmt 0:a0ab83db7fa0 79 led3 = !led1;
jeanmt 1:b71ac293907d 80 vitesse = float(Position2 - position)/(POS_PER_METER*0.020) ;
jeanmt 1:b71ac293907d 81 //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 82 position = Position2 ;
jeanmt 1:b71ac293907d 83 capteur_timer.reset() ;
jeanmt 0:a0ab83db7fa0 84 }
jeanmt 1:b71ac293907d 85
jeanmt 1:b71ac293907d 86
jeanmt 0:a0ab83db7fa0 87 set_position(ValServo);
jeanmt 0:a0ab83db7fa0 88 pwm_mot.write(ValMot);
jeanmt 1:b71ac293907d 89
jeanmt 1:b71ac293907d 90
jeanmt 1:b71ac293907d 91
jeanmt 1:b71ac293907d 92
jeanmt 1:b71ac293907d 93
jeanmt 1:b71ac293907d 94 if(can_timer.read_ms() > CAN_TEMPO ){
jeanmt 1:b71ac293907d 95
jeanmt 1:b71ac293907d 96 send_can(vitesse, position) ;
jeanmt 1:b71ac293907d 97 can_timer.reset() ;
jeanmt 1:b71ac293907d 98 }
jeanmt 0:a0ab83db7fa0 99
jeanmt 0:a0ab83db7fa0 100
jeanmt 0:a0ab83db7fa0 101
jeanmt 1:b71ac293907d 102 }
jeanmt 0:a0ab83db7fa0 103 }
jeanmt 0:a0ab83db7fa0 104
jeanmt 0:a0ab83db7fa0 105 /******* Régulation PID ********/
jeanmt 0:a0ab83db7fa0 106 // Ecart entre la consigne et la mesure
jeanmt 0:a0ab83db7fa0 107 // ecart = vref - vitesse;
jeanmt 0:a0ab83db7fa0 108
jeanmt 0:a0ab83db7fa0 109 // Terme proportionnel
jeanmt 0:a0ab83db7fa0 110 // P_x = Kp * ecart;
jeanmt 0:a0ab83db7fa0 111
jeanmt 0:a0ab83db7fa0 112 // Calcul de la commande
jeanmt 0:a0ab83db7fa0 113 // commande = P_x + I_x;
jeanmt 0:a0ab83db7fa0 114
jeanmt 0:a0ab83db7fa0 115 // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
jeanmt 0:a0ab83db7fa0 116 // I_x = I_x + Ki * dt * ecart;
jeanmt 0:a0ab83db7fa0 117 /******* Fin régulation PID ********/
jeanmt 0:a0ab83db7fa0 118
jeanmt 0:a0ab83db7fa0 119
jeanmt 0:a0ab83db7fa0 120
jeanmt 0:a0ab83db7fa0 121
jeanmt 1:b71ac293907d 122 void send_can (float vitesse, int nbr){
jeanmt 1:b71ac293907d 123 float res ;
jeanmt 1:b71ac293907d 124 int res2 ;
jeanmt 1:b71ac293907d 125 unsigned char *chptr ;
jeanmt 1:b71ac293907d 126 CANMessage message;
jeanmt 1:b71ac293907d 127 message.len=8;
jeanmt 1:b71ac293907d 128 chptr = (unsigned char *) & vitesse;
jeanmt 1:b71ac293907d 129 message.data[0] = (*chptr++);
jeanmt 1:b71ac293907d 130 message.data[1] = (*chptr++);
jeanmt 1:b71ac293907d 131 message.data[2] = (*chptr++);
jeanmt 1:b71ac293907d 132 message.data[3] = (*chptr);
jeanmt 1:b71ac293907d 133 message.data[4] = char(nbr) ;
jeanmt 1:b71ac293907d 134 message.data[5] = char(nbr>>8) ;
jeanmt 1:b71ac293907d 135 message.data[6] = char(nbr>>16) ;
jeanmt 1:b71ac293907d 136 message.data[7] = char(nbr>>24) ;
jeanmt 1:b71ac293907d 137 message.id=0x400;
jeanmt 1:b71ac293907d 138
jeanmt 1:b71ac293907d 139 res = Bytes2Float((char*)message.data, 0) ;
jeanmt 1:b71ac293907d 140 res2 = Bytes2Int((char*)message.data, 4) ;
jeanmt 1:b71ac293907d 141 usb.printf("\rvitesse = %f, pos = %d", vitesse, nbr) ;
jeanmt 1:b71ac293907d 142 message.type=CANData;
jeanmt 1:b71ac293907d 143 message.format=CANStandard;
jeanmt 1:b71ac293907d 144 can2.write(message);
jeanmt 1:b71ac293907d 145
jeanmt 1:b71ac293907d 146 }
jeanmt 1:b71ac293907d 147
jeanmt 1:b71ac293907d 148
jeanmt 0:a0ab83db7fa0 149
jeanmt 0:a0ab83db7fa0 150
jeanmt 0:a0ab83db7fa0 151 void set_position(unsigned short us)
jeanmt 0:a0ab83db7fa0 152 {
jeanmt 0:a0ab83db7fa0 153 unsigned short period= us+20000;
jeanmt 0:a0ab83db7fa0 154 float pulse_width;
jeanmt 0:a0ab83db7fa0 155 if (tempo.read_ms()> 50)
jeanmt 0:a0ab83db7fa0 156 {
jeanmt 0:a0ab83db7fa0 157 pulse_width=((float)us)/((float)period);
jeanmt 0:a0ab83db7fa0 158 Servo.period_us(period);
jeanmt 0:a0ab83db7fa0 159 Servo.write(pulse_width);
jeanmt 0:a0ab83db7fa0 160 tempo.reset();
jeanmt 0:a0ab83db7fa0 161 }
jeanmt 0:a0ab83db7fa0 162 }
jeanmt 1:b71ac293907d 163
jeanmt 1:b71ac293907d 164 int Bytes2Int(char data[], int position ){
jeanmt 1:b71ac293907d 165 int nbr = 0 ;
jeanmt 1:b71ac293907d 166 return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
jeanmt 1:b71ac293907d 167 }
jeanmt 1:b71ac293907d 168
jeanmt 1:b71ac293907d 169
jeanmt 1:b71ac293907d 170 float Float2Bytes( char data[], float nbr){
jeanmt 1:b71ac293907d 171 unsigned char* chptr ;
jeanmt 1:b71ac293907d 172 chptr = (unsigned char *) & nbr;
jeanmt 1:b71ac293907d 173 data[0]= (*chptr++);
jeanmt 1:b71ac293907d 174 data[1]= (*chptr++);
jeanmt 1:b71ac293907d 175 data[2]= (*chptr++);
jeanmt 1:b71ac293907d 176 data[3]= (*chptr);
jeanmt 1:b71ac293907d 177
jeanmt 1:b71ac293907d 178 }
jeanmt 1:b71ac293907d 179
jeanmt 1:b71ac293907d 180 float Bytes2Float( char data[], int position){
jeanmt 1:b71ac293907d 181 union {
jeanmt 1:b71ac293907d 182 float float_sent;
jeanmt 1:b71ac293907d 183 unsigned char bytes[4];
jeanmt 1:b71ac293907d 184 } u;
jeanmt 1:b71ac293907d 185 memcpy(u.bytes, data + position,4) ;
jeanmt 1:b71ac293907d 186 return u.float_sent ;
jeanmt 1:b71ac293907d 187 }