Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 39:de3638276b7e
- Parent:
- 38:dba82d8b08e2
- Child:
- 40:b9450d753782
--- a/stateMachines.cpp Wed Sep 19 11:51:26 2018 +0000
+++ b/stateMachines.cpp Wed Sep 19 12:56:05 2018 +0000
@@ -11,7 +11,7 @@
#endif
Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
-Timer timerSinceGetTachy;
+Timer timerSinceTachy;
double distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
double distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
@@ -42,7 +42,7 @@
int index_fifo_lidar = 0;
//sections
s_Section p_section1;
-//s_Section p_section2;
+s_Section p_section2;
//PWM Controls
PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4
@@ -96,7 +96,7 @@
//SPEED
double maxSpeed_cmps = 0;
double tachySpeed_cmps = 0; //en cm/s
-//double tachyStepsRegister = 0;
+double tachyStepsRegister = 0;
double tachySectionDist_cm = 0;
double tachyTotalDist_cm = 0.0;
@@ -191,12 +191,13 @@
#if DEBUG > 0
pc.printf("[START TO TRANSMIT]\r\n");
#endif
- serialLidar.printf("time,position 45,position 90,largeur pistepwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
+ serialLidar.printf("time,position 45,position 90,largeur piste,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
for(int p=0; p<indexSample; p++) {
- serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d,%d\r\n",
+ serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d\r\n",
history[p].time,
history[p].position45,
history[p].position90,
+ history[p].largeurPiste,
history[p].pwm_thro_us,
history[p].pwm_dir_us,
history[p].dist,
@@ -232,17 +233,12 @@
return;
}
-void getTachySpeed()
-{
-
-}
-
double getShortDistMoy(AnalogIn* p,double* tab,int size)
{
tab[index_fifo_ir] = 3.3 * (double)p->read(); // on convertit directement en volts
//tension proportionelle à l'inverse de la distance en dessous de 2V
- tab[index_fifo_ir] = 20.0/(tab[index_fifo_ir]-0.3);
+ tab[index_fifo_ir] = (11.3531/(tab[index_fifo_ir]-0.1034))-0.42;
int sumMoy = 0;
for(int k=0; k<size; k++) {
@@ -262,21 +258,30 @@
}
return sumMoy/size;
}
+void tachyCheck()
+{
+ if(timerSinceTachy.read_us() > 500000)
+ {
+ tachySpeed_cmps = 0.0;
+ }
+return;
+}
+
void it4cm()
{
- //faire un flag sur l'IT pour le freinage.
- //pour que si le biniou ne bouge plus, il n'essaye plus de freiner tant
- //qu'il n'y a pa un nouveau calcul de la vitesse.
-
+ //le timer sert de flag sur le freinage:
+ //si le biniou ne bouge plus, il n'essaye plus de freiner tant qu'il n'y a pas un nouveau calcul de la vitesse
+ //dans la fonction tachyCheck() >> quand il n'y a pas eu d'IT en 500 ms si la vitesse est inférieure à 0.16 m/s, elle sera considérée comme nulle.
+
tachySectionDist_cm += TACHY_CM;
tachyTotalDist_cm += TACHY_CM;
- tachySpeed_cmps = (TACHY_CM * 1000000.0)/(double)timerSinceGetTachy.read_us(); // a chaque IT on a parcouru 8 cm soit (8*1000000)/durée
+ tachySpeed_cmps = (TACHY_CM * 1000000.0)/(double)timerSinceTachy.read_us(); // a chaque IT on a parcouru 8 cm soit (8*1000000)/durée
#if DEBUG > 2
pc.printf("IT: distance parcourue %.4lf , vitesse:%.4lf \r\n",tachyTotalDist_cm,tachySpeed_cmps);
#endif
- timerSinceGetTachy.reset();
- timerSinceGetTachy.start();
+ timerSinceTachy.reset();
+ timerSinceTachy.start();
return;
#if DEBUG > 0
@@ -308,9 +313,7 @@
//########## INIT STATES MACHINES ##########
void mursInit(void)
{
-
- pc.baud(115200); // pourquoi c'est la ?
- #if DEBUG > 0
+#if DEBUG > 0
pc.printf("Init Murs\r\n");
#endif
timeSinceStart.start();
@@ -337,22 +340,33 @@
st_currentSection=ARRET;
p_sectionCourante=&p_section1;
it_tachymeter.fall(&it4cm);
- timerSinceGetTachy.start();
- //getTachySpeed();//to reset
- tachySectionDist_cm = 0.0;
- //tachyStepsRegister = 0;
+ timerSinceTachy.start();
+ tachySectionDist_cm = 0;
+ tachyStepsRegister = 0;
//section de test 1
p_section1.nextSection =NULL;// &p_section2;
p_section1.consigne_position = 75.0;
- p_section1.targetSpeed_cmps = 500.0;
+ p_section1.targetSpeed_cmps = 400.0;
p_section1.slowSpeed_cmps = 328.0;
p_section1.coef_p_speed = 1;
p_section1.lidarWarningDist_cm = 120.0;
- p_section1.lng_section_cm = 2000.0;//20m
- p_section1.coef_p = 0.40;
- p_section1.coef_i = 0.01;
- p_section1.coef_d = 0.05;
+ p_section1.lng_section_cm = 30000.0;//30m
+ p_section1.coef_p = 4;
+ p_section1.coef_i = 0.00001;
+ p_section1.coef_d = 0.00001;
+
+ //section de test
+ p_section2.nextSection = NULL;
+ p_section2.consigne_position = 75.0;
+ p_section2.targetSpeed_cmps = 328.0;
+ p_section2.slowSpeed_cmps = 328.0;
+ p_section2.coef_p_speed = 1.0;
+ p_section2.lidarWarningDist_cm = 300.0;
+ p_section2.lng_section_cm = 200.0;//2m
+ p_section2.coef_p = 1;
+ p_section2.coef_i = 0.00001;
+ p_section2.coef_d = 0.00001;
return;
}
@@ -409,15 +423,14 @@
distMurD45Moy = getDistMoy(&anaD45,distMurD45,NB_ECHANTILLONS_IR);
distMurG90Moy = getDistMoy(&anaG90,distMurG90,NB_ECHANTILLONS_IR);
distMurD90Moy = getDistMoy(&anaD90,distMurD90,NB_ECHANTILLONS_IR);
- //shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
- //shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
+ shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
+ shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
-// pour debug je supprime la fonction avec les courts et les longs
trueDistMurD90Moy = distMurD90Moy;
trueDistMurG90Moy = distMurG90Moy;
-/*
+
if(shortDistMurG90Moy < DIST_MIN_LONG_CM)
{
trueDistMurG90Moy = shortDistMurG90Moy;
@@ -431,7 +444,7 @@
}else
{
trueDistMurD90Moy = distMurD90Moy;
- }*/
+ }
#ifdef DLVV
@@ -529,7 +542,6 @@
#if (DEBUG > 3)
pc.printf("\r\nUpdate Throttle\r\n");
#endif
- //getTachySpeed();
switch (st_thro) {
case REGULATION_SPEED:
if( st_currentSection == ARRET ||
@@ -599,20 +611,26 @@
largeurPiste90 = 150.0;
positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM );
positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
+#ifdef OMAR
pc.printf("REF_A_GAUCHE\r\n");
+#endif
break;
case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche
largeurPiste90 = 150.0;
positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM;
+#ifdef OMAR
pc.printf("REF_A_DROITE\r\n");
+#endif
break;
default://REF_BIDIR
largeurPiste90 = trueDistMurG90Moy + trueDistMurD90Moy ;
largeurPiste45 = distMurG45Moy + distMurD45Moy ;
positionSurPiste90 = (trueDistMurG90Moy );
positionSurPiste45 = (distMurG45Moy);
- pc.printf("default\r\n");
+#ifdef OMAR
+ pc.printf("REF_BIDIR\r\n");
+#endif
break;
}
@@ -620,42 +638,32 @@
//deriv correction
derive45 = positionSurPiste45 - positionSurPiste45Prev;
derive90 = positionSurPiste90 - positionSurPiste90Prev;
-
- //pc.printf("derive45 => %.4lf \r\n ",derive45);
- // pc.printf("derive90 => %.4lf \r\n ",derive90);
- /*
- pc.printf("distMurG45Moy => %.4lf \r\n ",distMurG45Moy);
- pc.printf("distMurD45Moy => %.4lf \r\n ",distMurD45Moy);
- pc.printf("trueDistMurG90Moy => %.4lf \r\n ",trueDistMurG90Moy);
- pc.printf("trueDistMurD90Moy => %.4lf \r\n ",trueDistMurD90Moy);
- pc.printf("positionSurPiste90 => %.4lf \r\n ",positionSurPiste90);
- pc.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
- pc.printf("largeurPiste90 => %.4lf \r\n ",largeurPiste90);
- */
-
-
+
+#ifdef OMAR/*
+ pc.printf("derive45 => %.4lf \r\n ",derive45);
+ pc.printf("derive90 => %.4lf \r\n ",derive90);
+ pc.printf("distMurG45Moy => %.4lf \r\n ",distMurG45Moy);
+ pc.printf("distMurD45Moy => %.4lf \r\n ",distMurD45Moy);
+ pc.printf("trueDistMurG90Moy => %.4lf \r\n ",trueDistMurG90Moy);
+ pc.printf("trueDistMurD90Moy => %.4lf \r\n ",trueDistMurD90Moy);
+ pc.printf("positionSurPiste90 => %.4lf \r\n ",positionSurPiste90);
+ pc.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
+ pc.printf("largeurPiste90 => %.4lf \r\n ",largeurPiste90);*/
+#endif
+
//integral correction
- //lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
- // integralSum=0;
- //for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
- // integralSum+=lastDifferences90[f];
- // }
- //integral correction
- integralSum += (positionSurPiste90 - (largeurPiste90/2.0));
-
-
-
+ lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
+ integralSum=0;
+ for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
+ integralSum+=lastDifferences90[f];
+ }
+ lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
+
+
+
//application des coefficients
- pulseDirection_us_temp = (
- ((positionSurPiste45 - (largeurPiste45/2.0)) * p_sectionCourante->coef_p)
- + ((positionSurPiste90 - (largeurPiste90/2.0)) * p_sectionCourante->coef_p)
- + ( -derive45 * p_sectionCourante->coef_d)
- + ( -derive90 * p_sectionCourante->coef_d)
- + ( integralSum * p_sectionCourante->coef_i)
- )* (300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
+ pulseDirection_us_temp = ((positionSurPiste45 - (largeurPiste45/2.0)) * 4)
+ DIRECTION_PULSE_MIDDLE;
-
-
/*pulseDirection_us_temp = COEF_RAYON_BR_S_DIST/tachySpeed_cmps * //facteur de vitesse: plus on va vite, moins on corrige
(
( (p_sectionCourante->consigne_position - positionSurPiste45) * p_sectionCourante->coef_p)
@@ -665,10 +673,12 @@
+ DIRECTION_PULSE_MIDDLE;*/
//pc.printf("\r %.4lf ",pulseDirection_us);
pulseDirection_us = (int)pulseDirection_us_temp ;
+#ifdef OMAR
pc.printf("p_sectionCourante->consigne_position => %.4lf \r\n ",p_sectionCourante->consigne_position);
pc.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
pc.printf("p_sectionCourante->coef_p => %.4lf \r\n ",p_sectionCourante->coef_p);
pc.printf("pulseDirection_us_temp => %.4lf \r\n ",pulseDirection_us_temp);
+#endif
//gestioon du dépassement
if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
#if DEBUG > 1