Gaëtan Andrieu / PID_boussole_

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.

Dependents:   I2Cboussole_avec_librairie Projet__tutore

PIDboussole.cpp@0:4e5a5b017550, 2019-04-01 (annotated)

Committer:
Gaetan_Andrieu
Date:
Mon Apr 01 16:16:32 2019 +0000
Revision:
0:4e5a5b017550
Child:
1:1d8f9c73cdc4
projet;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaetan_Andrieu 0:4e5a5b017550 1 #include "PIDboussole.h"
Gaetan_Andrieu 0:4e5a5b017550 2
Gaetan_Andrieu 0:4e5a5b017550 3 #define T 0.03//0.05
Gaetan_Andrieu 0:4e5a5b017550 4 #define Bkp 2
Gaetan_Andrieu 0:4e5a5b017550 5 #define Bki 0
Gaetan_Andrieu 0:4e5a5b017550 6 #define Bkd 0
Gaetan_Andrieu 0:4e5a5b017550 7 DigitalOut LED(PA_5);
Gaetan_Andrieu 0:4e5a5b017550 8
Gaetan_Andrieu 0:4e5a5b017550 9 PID_boussole::PID_boussole(Motor* _moteur_d, Motor* _moteur_g, CMPS03* _boussole)
Gaetan_Andrieu 0:4e5a5b017550 10 {
Gaetan_Andrieu 0:4e5a5b017550 11 moteur_g = _moteur_g;
Gaetan_Andrieu 0:4e5a5b017550 12 moteur_d = _moteur_d;
Gaetan_Andrieu 0:4e5a5b017550 13 boussole = _boussole;
Gaetan_Andrieu 0:4e5a5b017550 14 angle = 0;
Gaetan_Andrieu 0:4e5a5b017550 15 V_tout_droit = 1;
Gaetan_Andrieu 0:4e5a5b017550 16 tickPID_boussole.attach(callback(this,&PID_boussole::fPID_boussole), T);
Gaetan_Andrieu 0:4e5a5b017550 17 }
Gaetan_Andrieu 0:4e5a5b017550 18
Gaetan_Andrieu 0:4e5a5b017550 19 void PID_boussole::fPID_boussole()
Gaetan_Andrieu 0:4e5a5b017550 20 {
Gaetan_Andrieu 0:4e5a5b017550 21 angle = boussole->getBearing();
Gaetan_Andrieu 0:4e5a5b017550 22 Cpre = Cact;
Gaetan_Andrieu 0:4e5a5b017550 23 Cact = angle;
Gaetan_Andrieu 0:4e5a5b017550 24 Csomme_erreur += Cconsigne - Cact;
Gaetan_Andrieu 0:4e5a5b017550 25 Cnouvelle_consigne = Bkp*(Cconsigne - Cact) + Bkd*(Cpre - Cact) + Bki*Csomme_erreur;
Gaetan_Andrieu 0:4e5a5b017550 26 //V_tourne = (0.4-(abs(Cnouvelle_consigne)/(360*4)));
Gaetan_Andrieu 0:4e5a5b017550 27 V_tourne = abs(angle-Cconsigne)/(360)+0.4;
Gaetan_Andrieu 0:4e5a5b017550 28 if(V_tourne>1) V_tourne=1;
Gaetan_Andrieu 0:4e5a5b017550 29
Gaetan_Andrieu 0:4e5a5b017550 30 if(angle<(Cconsigne+10) && angle>(Cconsigne-10))
Gaetan_Andrieu 0:4e5a5b017550 31 {
Gaetan_Andrieu 0:4e5a5b017550 32 moteur_g->stop(0.0);
Gaetan_Andrieu 0:4e5a5b017550 33 moteur_d->stop(0.0);
Gaetan_Andrieu 0:4e5a5b017550 34 }
Gaetan_Andrieu 0:4e5a5b017550 35 else
Gaetan_Andrieu 0:4e5a5b017550 36 {
Gaetan_Andrieu 0:4e5a5b017550 37 if((Cconsigne - Cact) <= 0)
Gaetan_Andrieu 0:4e5a5b017550 38 {
Gaetan_Andrieu 0:4e5a5b017550 39 //moteur_g->speed(V_tourne-0.1);
Gaetan_Andrieu 0:4e5a5b017550 40 moteur_g->speed(V_tourne);
Gaetan_Andrieu 0:4e5a5b017550 41 //moteur_d->speed(-V_tout_droit);
Gaetan_Andrieu 0:4e5a5b017550 42 moteur_d->speed(0);
Gaetan_Andrieu 0:4e5a5b017550 43 LED = 0;
Gaetan_Andrieu 0:4e5a5b017550 44 }
Gaetan_Andrieu 0:4e5a5b017550 45 else
Gaetan_Andrieu 0:4e5a5b017550 46 {
Gaetan_Andrieu 0:4e5a5b017550 47 //moteur_g->speed(-V_tout_droit);
Gaetan_Andrieu 0:4e5a5b017550 48 moteur_g->speed(0);
Gaetan_Andrieu 0:4e5a5b017550 49 moteur_d->speed(V_tourne+0.1);
Gaetan_Andrieu 0:4e5a5b017550 50 LED = 1;
Gaetan_Andrieu 0:4e5a5b017550 51 }
Gaetan_Andrieu 0:4e5a5b017550 52 }
Gaetan_Andrieu 0:4e5a5b017550 53 }
Gaetan_Andrieu 0:4e5a5b017550 54
Gaetan_Andrieu 0:4e5a5b017550 55 void PID_boussole::STOP_PID_boussole()
Gaetan_Andrieu 0:4e5a5b017550 56 {
Gaetan_Andrieu 0:4e5a5b017550 57 tickPID_boussole.detach();
Gaetan_Andrieu 0:4e5a5b017550 58 }
Gaetan_Andrieu 0:4e5a5b017550 59
Gaetan_Andrieu 0:4e5a5b017550 60 void PID_boussole::START_PID_boussole()
Gaetan_Andrieu 0:4e5a5b017550 61 {
Gaetan_Andrieu 0:4e5a5b017550 62 tickPID_boussole.attach(callback(this,&PID_boussole::fPID_boussole), T);
Gaetan_Andrieu 0:4e5a5b017550 63 }
Gaetan_Andrieu 0:4e5a5b017550 64
Gaetan_Andrieu 0:4e5a5b017550 65 void PID_boussole::PID_boussole_consigne(int cons)
Gaetan_Andrieu 0:4e5a5b017550 66 {
Gaetan_Andrieu 0:4e5a5b017550 67 Cconsigne = cons;
Gaetan_Andrieu 0:4e5a5b017550 68 }