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
- Committer:
- QuentinD
- Date:
- 2021-04-15
- Revision:
- 2:ce23bd78a43a
- Parent:
- 1:b71ac293907d
File content as of revision 2:ce23bd78a43a:
// Display changes in encoder position and direction #include "mbed.h" #include "qeihw.h" #define PLAGE_ANGULAIRE 800 // 800 // 420 #define GAUCHE_MAX 500 // 500 /// 720 #define TOUT_DROIT 900 #define ARRET_MOTEUR_NON_VOULU 3 #define BLINKING_RATE_MS #define CAPTEUR_TEMPO 200 #define CAN_TEMPO 50 #define MOTEUR_TEMPO 50 #include "mbed.h" #define POS_PER_METER 43636 //#define GAUCHE_MAX 710 //#define PLAGE_ANGULAIRE 420 CAN can2(p30, p29,1000000); PwmOut Servo(p23); PwmOut pwm_mot(p26); Serial usb(USBTX,USBRX,115200) ; DigitalOut inA(p24); DigitalOut inB(p25); DigitalOut led1(LED1); DigitalOut led3(LED3); void print (void); void serial_ISR(void); int Bytes2Int(char data[], int position ) ; void set_position(unsigned short us) ; void send_can (float vitesse, int nbr) ; void send_can2(bool Moteur_stopper); float Bytes2Float( char data[], int position) ; float Float2Bytes( char data[], float nbr) ; QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE ); Timer tempo ; Timer can_timer ; Timer mot_timer ; Timer capteur_timer ; Timer Probleme_arret; unsigned short pos = 1000; float vit = 0 ; int main() { int32_t Position2, position = 0 , temp = 0, premier_probleme = 0; float vitesse = 1.05 , direction , ValMot, ValServo ; unsigned char *chptr; char Sens = 1, Moteur_stopper = 0; int ajustement_direction = 0; CANMessage msg_rx; CANMessage msg_tx; usb.attach(&serial_ISR,Serial::RxIrq); qei.SetDigiFilter(10); qei.SetMaxPosition(0xFFFFFFFF); mot_timer.start() ; can_timer.start() ; Probleme_arret.start(); capteur_timer.start() ; tempo.start() ; Servo.period_ms(20); pwm_mot.period_us(50); inA = 1; inB = 0; while(1) { if (can2.read(msg_rx)) { ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX; if(ValServo>=TOUT_DROIT) { ajustement_direction = (ValServo - TOUT_DROIT)*0.33; ValServo = ValServo - ajustement_direction; } ValMot = float(msg_rx.data[1]/255.0f); Sens = msg_rx.data[2] ; inA = Sens; inB = !Sens; } if (capteur_timer.read_ms() > CAPTEUR_TEMPO){ Position2 = qei.GetPosition(); led1 = qei.Direction() == SET ? 1 : 0 ; led3 = !led1; vitesse = float(Position2 - position)/(POS_PER_METER*0.020) ; //usb.printf("la pwm est de %f la position est de %lu la vitesse est de %f km/h\r",ValMot, Position2,vitesse); position = Position2 ; capteur_timer.reset() ; } set_position(ValServo); pwm_mot.write(ValMot); //set_position(pos); //pwm_mot.write(vit); if(can_timer.read_ms() > CAN_TEMPO ) { if (vitesse <= ARRET_MOTEUR_NON_VOULU) { if(premier_probleme == 0) premier_probleme = Probleme_arret.read_ms(); if(Probleme_arret.read_ms() >= premier_probleme + 5000); Moteur_stopper = 1; if(vitesse >= ARRET_MOTEUR_NON_VOULU) { Moteur_stopper = 0; premier_probleme = 0; } } send_can(vitesse, position) ; send_can2(Moteur_stopper); can_timer.reset() ; } } } /******* Régulation PID ********/ // Ecart entre la consigne et la mesure // ecart = vref - vitesse; // Terme proportionnel // P_x = Kp * ecart; // Calcul de la commande // commande = P_x + I_x; // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant) // I_x = I_x + Ki * dt * ecart; /******* Fin régulation PID ********/ void send_can (float vitesse, int nbr){ float res ; int res2 ; unsigned char *chptr ; CANMessage message; message.len=8; chptr = (unsigned char *) & vitesse; message.data[0] = (*chptr++); message.data[1] = (*chptr++); message.data[2] = (*chptr++); message.data[3] = (*chptr); message.data[4] = char(nbr) ; message.data[5] = char(nbr>>8) ; message.data[6] = char(nbr>>16) ; message.data[7] = char(nbr>>24) ; message.id=0x400; res = Bytes2Float((char*)message.data, 0) ; res2 = Bytes2Int((char*)message.data, 4) ; usb.printf("\rvitesse = %f, pos = %d", vitesse, nbr) ; message.type=CANData; message.format=CANStandard; can2.write(message); } void send_can2(bool Moteur_stopper) { CANMessage message2; message2.len=1; message2.data[0] = bool(Moteur_stopper); message2.id=0x500; usb.printf("\rARRET MOTEUR %d",Moteur_stopper); message2.type=CANData; message2.format=CANStandard; can2.write(message2); } void set_position(unsigned short us) { unsigned short period= us+20000; float pulse_width; if (tempo.read_ms() > 50) { pulse_width=((float)us)/((float)period); Servo.period_us(period); Servo.write(pulse_width); tempo.reset(); } } int Bytes2Int(char data[], int position ){ int nbr = 0 ; return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ; } float Float2Bytes( char data[], float nbr){ unsigned char* chptr ; chptr = (unsigned char *) & nbr; data[0]= (*chptr++); data[1]= (*chptr++); data[2]= (*chptr++); data[3]= (*chptr); } float Bytes2Float( char data[], int position){ union { float float_sent; unsigned char bytes[4]; } u; memcpy(u.bytes, data + position,4) ; return u.float_sent ; } void serial_ISR(void){ char msg=usb.getc(); switch (msg){ case 'a': pos=pos+100; break; case 'z': if (pos>100) pos=pos-100; break; case 'e': pos=pos+10; break; case 'r': if (pos>10) pos=pos-10; break; case 'q': if (vit<1.0) vit+=0.1 ; break; case 's': if (vit>0.0) vit-= 0.1 ; break; case 'd': if (vit<1.0) vit+=0.01 ; break; case 'f': if (vit>0.0) vit-=0.01 ; break; } usb.printf("pos = %d us vit = %f \n",pos,vit); }