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@1:1d8f9c73cdc4, 2019-04-03 (annotated)

Committer:
Gaetan_Andrieu
Date:
Wed Apr 03 22:28:57 2019 +0000
Revision:
1:1d8f9c73cdc4
Parent:
0:4e5a5b017550 
Robot par bluetooth

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 1:1d8f9c73cdc4 3 #define T 0.03
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 1:1d8f9c73cdc4 7 #define ANGLE_TOLERANCE 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 1:1d8f9c73cdc4 16 }
Gaetan_Andrieu 1:1d8f9c73cdc4 17
Gaetan_Andrieu 1:1d8f9c73cdc4 18 void PID_boussole::aller_tout_droit()
Gaetan_Andrieu 1:1d8f9c73cdc4 19 {
Gaetan_Andrieu 1:1d8f9c73cdc4 20 moteur_g->speed(1);
Gaetan_Andrieu 1:1d8f9c73cdc4 21 moteur_d->speed(0.76);
Gaetan_Andrieu 0:4e5a5b017550 22 }
Gaetan_Andrieu 0:4e5a5b017550 23
Gaetan_Andrieu 0:4e5a5b017550 24 void PID_boussole::fPID_boussole()
Gaetan_Andrieu 0:4e5a5b017550 25 {
Gaetan_Andrieu 1:1d8f9c73cdc4 26 if(abs(Cconsigne)<=360)
Gaetan_Andrieu 0:4e5a5b017550 27 {
Gaetan_Andrieu 1:1d8f9c73cdc4 28 angle = boussole->getBearing();
Gaetan_Andrieu 1:1d8f9c73cdc4 29 Cact = angle;
Gaetan_Andrieu 1:1d8f9c73cdc4 30
Gaetan_Andrieu 1:1d8f9c73cdc4 31 V_tourne = abs(angle-Cconsigne)/(360.0)+0.55;
Gaetan_Andrieu 1:1d8f9c73cdc4 32 if(V_tourne>1) V_tourne=1;
Gaetan_Andrieu 1:1d8f9c73cdc4 33
Gaetan_Andrieu 1:1d8f9c73cdc4 34 if(angle<(Cconsigne+ANGLE_TOLERANCE) && angle>(Cconsigne-ANGLE_TOLERANCE))
Gaetan_Andrieu 0:4e5a5b017550 35 {
Gaetan_Andrieu 1:1d8f9c73cdc4 36 aller_tout_droit();
Gaetan_Andrieu 1:1d8f9c73cdc4 37 flag_bon_sens = 1;
Gaetan_Andrieu 0:4e5a5b017550 38 }
Gaetan_Andrieu 0:4e5a5b017550 39 else
Gaetan_Andrieu 0:4e5a5b017550 40 {
Gaetan_Andrieu 1:1d8f9c73cdc4 41 flag_bon_sens = 0;
Gaetan_Andrieu 1:1d8f9c73cdc4 42 if((Cconsigne - Cact) <= 0)
Gaetan_Andrieu 1:1d8f9c73cdc4 43 {
Gaetan_Andrieu 1:1d8f9c73cdc4 44 moteur_g->speed(V_tourne);
Gaetan_Andrieu 1:1d8f9c73cdc4 45 moteur_d->speed(0);
Gaetan_Andrieu 1:1d8f9c73cdc4 46 }
Gaetan_Andrieu 1:1d8f9c73cdc4 47 else
Gaetan_Andrieu 1:1d8f9c73cdc4 48 {
Gaetan_Andrieu 1:1d8f9c73cdc4 49 moteur_g->speed(0);
Gaetan_Andrieu 1:1d8f9c73cdc4 50 moteur_d->speed(V_tourne+0.1);
Gaetan_Andrieu 1:1d8f9c73cdc4 51 }
Gaetan_Andrieu 1:1d8f9c73cdc4 52 }
Gaetan_Andrieu 1:1d8f9c73cdc4 53 }
Gaetan_Andrieu 1:1d8f9c73cdc4 54 else flag_bon_sens = 1;
Gaetan_Andrieu 1:1d8f9c73cdc4 55
Gaetan_Andrieu 0:4e5a5b017550 56 }
Gaetan_Andrieu 0:4e5a5b017550 57
Gaetan_Andrieu 0:4e5a5b017550 58 void PID_boussole::STOP_PID_boussole()
Gaetan_Andrieu 0:4e5a5b017550 59 {
Gaetan_Andrieu 0:4e5a5b017550 60 tickPID_boussole.detach();
Gaetan_Andrieu 0:4e5a5b017550 61 }
Gaetan_Andrieu 0:4e5a5b017550 62
Gaetan_Andrieu 0:4e5a5b017550 63 void PID_boussole::START_PID_boussole()
Gaetan_Andrieu 0:4e5a5b017550 64 {
Gaetan_Andrieu 0:4e5a5b017550 65 tickPID_boussole.attach(callback(this,&PID_boussole::fPID_boussole), T);
Gaetan_Andrieu 0:4e5a5b017550 66 }
Gaetan_Andrieu 0:4e5a5b017550 67
Gaetan_Andrieu 0:4e5a5b017550 68 void PID_boussole::PID_boussole_consigne(int cons)
Gaetan_Andrieu 0:4e5a5b017550 69 {
Gaetan_Andrieu 0:4e5a5b017550 70 Cconsigne = cons;
Gaetan_Andrieu 0:4e5a5b017550 71 }