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@0:a0ab83db7fa0, 2021-03-30 (annotated)

Committer:
jeanmt
Date:
Tue Mar 30 15:45:05 2021 +0000
Revision:
0:a0ab83db7fa0
Child:
1:b71ac293907d
programme carte moteur (fonctionnel)

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 0:a0ab83db7fa0 7 #include "mbed.h"
jeanmt 0:a0ab83db7fa0 8
jeanmt 0:a0ab83db7fa0 9 CAN can2(p30, p29,1000000);
jeanmt 0:a0ab83db7fa0 10
jeanmt 0:a0ab83db7fa0 11 PwmOut Servo(p23);
jeanmt 0:a0ab83db7fa0 12 PwmOut pwm_mot(p26);
jeanmt 0:a0ab83db7fa0 13
jeanmt 0:a0ab83db7fa0 14 DigitalOut inA(p24);
jeanmt 0:a0ab83db7fa0 15 DigitalOut inB(p25);
jeanmt 0:a0ab83db7fa0 16 DigitalOut led1(LED1);
jeanmt 0:a0ab83db7fa0 17 DigitalOut led3(LED3);
jeanmt 0:a0ab83db7fa0 18
jeanmt 0:a0ab83db7fa0 19 void set_position(unsigned short us);
jeanmt 0:a0ab83db7fa0 20
jeanmt 0:a0ab83db7fa0 21
jeanmt 0:a0ab83db7fa0 22
jeanmt 0:a0ab83db7fa0 23 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
jeanmt 0:a0ab83db7fa0 24 Timer tempo ;
jeanmt 0:a0ab83db7fa0 25 Timer t;
jeanmt 0:a0ab83db7fa0 26 Timer can_timer ;
jeanmt 0:a0ab83db7fa0 27
jeanmt 0:a0ab83db7fa0 28 int main() {
jeanmt 0:a0ab83db7fa0 29
jeanmt 0:a0ab83db7fa0 30 can_timer.start() ;
jeanmt 0:a0ab83db7fa0 31 tempo.start() ;
jeanmt 0:a0ab83db7fa0 32 float vitesse , direction;
jeanmt 0:a0ab83db7fa0 33 unsigned char *chptr;
jeanmt 0:a0ab83db7fa0 34 float ValMot,ValServo;
jeanmt 0:a0ab83db7fa0 35 char Sens;
jeanmt 0:a0ab83db7fa0 36 DigitalOut led(LED1);
jeanmt 0:a0ab83db7fa0 37 CANMessage msg_rx;
jeanmt 0:a0ab83db7fa0 38 CANMessage msg_tx;
jeanmt 0:a0ab83db7fa0 39
jeanmt 0:a0ab83db7fa0 40 int32_t Position2, position , temp = 0;
jeanmt 0:a0ab83db7fa0 41
jeanmt 0:a0ab83db7fa0 42 //serial_port.attach(&serial_ISR,Serial::RxIrq);
jeanmt 0:a0ab83db7fa0 43 qei.SetDigiFilter(10);
jeanmt 0:a0ab83db7fa0 44 qei.SetMaxPosition(0xFFFFFFFF);
jeanmt 0:a0ab83db7fa0 45 t.start();
jeanmt 0:a0ab83db7fa0 46 msg_tx.id = 400 ;
jeanmt 0:a0ab83db7fa0 47 msg_tx.len = 4 ;
jeanmt 0:a0ab83db7fa0 48
jeanmt 0:a0ab83db7fa0 49 Servo.period_ms(20);
jeanmt 0:a0ab83db7fa0 50 pwm_mot.period_ms(20);
jeanmt 0:a0ab83db7fa0 51 while(1)
jeanmt 0:a0ab83db7fa0 52 { wait_ms(50) ;
jeanmt 0:a0ab83db7fa0 53 if (can2.read(msg_rx))
jeanmt 0:a0ab83db7fa0 54 {
jeanmt 0:a0ab83db7fa0 55 ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;;
jeanmt 0:a0ab83db7fa0 56 ValMot = float(msg_rx.data[1]/255.0f);
jeanmt 0:a0ab83db7fa0 57 Sens = msg_rx.data[2] ;
jeanmt 0:a0ab83db7fa0 58 inA = Sens;
jeanmt 0:a0ab83db7fa0 59 inB = !Sens;
jeanmt 0:a0ab83db7fa0 60
jeanmt 0:a0ab83db7fa0 61 }
jeanmt 0:a0ab83db7fa0 62
jeanmt 0:a0ab83db7fa0 63 if ( temp%200 == 0 ){
jeanmt 0:a0ab83db7fa0 64 Position2 = qei.GetPosition();
jeanmt 0:a0ab83db7fa0 65 led1 = qei.Direction() == SET ? 1 : 0;
jeanmt 0:a0ab83db7fa0 66 led3 = !led1;
jeanmt 0:a0ab83db7fa0 67 vitesse = (Position2 - position);
jeanmt 0:a0ab83db7fa0 68 vitesse = (vitesse/42000.0f);
jeanmt 0:a0ab83db7fa0 69 vitesse = (vitesse /(200/1000.0f));
jeanmt 0:a0ab83db7fa0 70 //printf("la pwm est de %f la position est de %lu la vitesse est de %f KM/H\n\r",pwm, Position2,(vitesse*3.6));
jeanmt 0:a0ab83db7fa0 71 position = Position2 ;
jeanmt 0:a0ab83db7fa0 72 }
jeanmt 0:a0ab83db7fa0 73
jeanmt 0:a0ab83db7fa0 74 set_position(ValServo);
jeanmt 0:a0ab83db7fa0 75 pwm_mot.write(ValMot);
jeanmt 0:a0ab83db7fa0 76
jeanmt 0:a0ab83db7fa0 77
jeanmt 0:a0ab83db7fa0 78
jeanmt 0:a0ab83db7fa0 79
jeanmt 0:a0ab83db7fa0 80 }
jeanmt 0:a0ab83db7fa0 81 }
jeanmt 0:a0ab83db7fa0 82
jeanmt 0:a0ab83db7fa0 83 /******* Régulation PID ********/
jeanmt 0:a0ab83db7fa0 84 // Ecart entre la consigne et la mesure
jeanmt 0:a0ab83db7fa0 85 // ecart = vref - vitesse;
jeanmt 0:a0ab83db7fa0 86
jeanmt 0:a0ab83db7fa0 87 // Terme proportionnel
jeanmt 0:a0ab83db7fa0 88 // P_x = Kp * ecart;
jeanmt 0:a0ab83db7fa0 89
jeanmt 0:a0ab83db7fa0 90 // Calcul de la commande
jeanmt 0:a0ab83db7fa0 91 // commande = P_x + I_x;
jeanmt 0:a0ab83db7fa0 92
jeanmt 0:a0ab83db7fa0 93 // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
jeanmt 0:a0ab83db7fa0 94 // I_x = I_x + Ki * dt * ecart;
jeanmt 0:a0ab83db7fa0 95 /******* Fin régulation PID ********/
jeanmt 0:a0ab83db7fa0 96
jeanmt 0:a0ab83db7fa0 97
jeanmt 0:a0ab83db7fa0 98
jeanmt 0:a0ab83db7fa0 99
jeanmt 0:a0ab83db7fa0 100
jeanmt 0:a0ab83db7fa0 101
jeanmt 0:a0ab83db7fa0 102
jeanmt 0:a0ab83db7fa0 103 void set_position(unsigned short us)
jeanmt 0:a0ab83db7fa0 104 {
jeanmt 0:a0ab83db7fa0 105 unsigned short period= us+20000;
jeanmt 0:a0ab83db7fa0 106 float pulse_width;
jeanmt 0:a0ab83db7fa0 107 if (tempo.read_ms()> 50)
jeanmt 0:a0ab83db7fa0 108 {
jeanmt 0:a0ab83db7fa0 109 pulse_width=((float)us)/((float)period);
jeanmt 0:a0ab83db7fa0 110 Servo.period_us(period);
jeanmt 0:a0ab83db7fa0 111 Servo.write(pulse_width);
jeanmt 0:a0ab83db7fa0 112 tempo.reset();
jeanmt 0:a0ab83db7fa0 113 }
jeanmt 0:a0ab83db7fa0 114 }