Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 52:228703200e35
- Parent:
- 51:09ecba68d0cf
--- a/stateMachines.cpp Fri Sep 21 13:17:24 2018 +0000
+++ b/stateMachines.cpp Fri Sep 21 17:29:30 2018 +0000
@@ -86,6 +86,8 @@
//LIDAR
Serial serialLidar(PC_10,PC_11); // tx, rx
+Serial serialDebug(PC_12,PD_2); // tx, rx
+
int distLidar;// LiDAR actually measured distance value
int distLidarPrev;
int strengthLidar;// LiDAR signal strength
@@ -360,34 +362,49 @@
tachyStepsRegister = 0;
//section de test 1
- p_section1.nextSection =&p_section3;// &p_section2;
+ p_section1.nextSection =&p_section2;// &p_section2;
p_section1.consigne_position = 75.0;
- p_section1.targetSpeed_cmps = 400.0;
+ p_section1.targetSpeed_cmps = 600.0;
p_section1.slowSpeed_cmps = 328.0;
p_section1.coef_p_speed = 1;
p_section1.lidarWarningDist_cm = 120.0;
- p_section1.lng_section_cm = 1000.0;//10m
- p_section1.coef_p90 = 4.0;
- p_section1.coef_d90 = 0.0;
+ p_section1.lng_section_cm = 1300.0;//13m
+ // coef P
+ p_section1.coef_pd90 = -0.03;
+ p_section1.coef_pg90 = 0.03;
+ p_section1.coef_pd45 = -0.03;
+ p_section1.coef_pg45 = 0.03;
+ // le reste
+ p_section1.coef_dd90 = 0.0;
+ p_section1.coef_dg90 = 0.0;
+ p_section1.coef_dd45 = 0.0;
+ p_section1.coef_dg45 = 0.0;
+
p_section1.coef_i90 = 0.0;
- p_section1.coef_p45 = 0.0;
- p_section1.coef_d45 = 0.0;
p_section1.coef_i45 = 0.0;
//section de test
p_section2.nextSection = &p_section3;
p_section2.consigne_position = 75.0;
- p_section2.targetSpeed_cmps = 328.0;
+ p_section2.targetSpeed_cmps = 400.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_p90 = 1.0;
- p_section2.coef_d90 = 0.0;
+ p_section2.lng_section_cm = 600.0;//2m
+ // coef en P
+ p_section2.coef_pd90 = -0.03;
+ p_section2.coef_pg90 = 0.03;
+ p_section2.coef_pd45 = -0.03;
+ p_section2.coef_pg45 = 0.03;
+ // le reste
+ p_section2.coef_dd90 = 0.0;
+ p_section2.coef_dg90 = 0.0;
+ p_section2.coef_dd45 = 0.0;
+ p_section2.coef_dg45 = 0.0;
+
p_section2.coef_i90 = 0.0;
- p_section2.coef_p45 = 1.0;
- p_section2.coef_d45 = 0.0;
p_section2.coef_i45 = 0.0;
+
p_section3.nextSection = NULL;
p_section3.consigne_position = 75.0;
@@ -395,13 +412,20 @@
p_section3.slowSpeed_cmps = 0.0;
p_section3.coef_p_speed = 1.0;
p_section3.lidarWarningDist_cm = 300.0;
- p_section3.lng_section_cm = 200.0;//2m
- p_section3.coef_p90 = 1.0;
- p_section3.coef_d90 = 0.0;
+ p_section3.lng_section_cm = 100.0;//2m
+ // coef en P
+ p_section3.coef_pd90 = -0.03;
+ p_section3.coef_pg90 = 0.03;
+ p_section3.coef_pd45 = -0.03;
+ p_section3.coef_pg45 = 0.03;
+
+ p_section3.coef_dd90 = 0.0;
+ p_section3.coef_dg90 = 0.0;
+ p_section3.coef_dd45 = 0.0;
+ p_section3.coef_dg45 = 0.0;
+
+ p_section3.coef_i45 = 0.0;
p_section3.coef_i90 = 0.0;
- p_section3.coef_p45 = 1.0;
- p_section3.coef_d45 = 0.0;
- p_section3.coef_i45 = 0.0;
return;
}
@@ -414,6 +438,7 @@
st_maxSpeed=SPEED_MAX;
maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
serialLidar.baud(115200);
+ serialDebug.baud(115200);
serialLidar.attach(&it_serial);
return;
}
@@ -458,14 +483,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;
trueDistMurD90Moy = distMurD90Moy;
trueDistMurG90Moy = distMurG90Moy;
-
+/*
if(shortDistMurG90Moy < DIST_MIN_LONG_CM) {
trueDistMurG90Moy = shortDistMurG90Moy;
} else {
@@ -476,7 +501,7 @@
} else {
trueDistMurD90Moy = distMurD90Moy;
}
-
+*/
#ifdef DLVV
switch (st_obstacle) {
@@ -639,6 +664,17 @@
//pour dériver
positionSurPiste45Prev = positionSurPiste45;
positionSurPiste90Prev = positionSurPiste90;
+
+ // distMurG45MoyPrev = distMurG45Moy ;
+ // distMurD45MoyPrev = distMurD45Moy ;
+ // distMurD45MoyPrev = distMurD45Moy ;
+ // distMurD45MoyPrev = distMurD45Moy ;
+
+
+
+
+
+
#if (DEBUG > 3)
pc.printf("\r\n Output MURS\r\n");
#endif
@@ -676,17 +712,8 @@
derive45 = positionSurPiste45 - positionSurPiste45Prev;
derive90 = positionSurPiste90 - positionSurPiste90Prev;
-#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);
@@ -702,13 +729,34 @@
//application des coefficients
pulseDirection_us = (int)(
- ((positionSurPiste45 - (largeurPiste45/2)) * p_sectionCourante->coef_p45)
- + ((positionSurPiste90 - (largeurPiste90/2)) * p_sectionCourante->coef_p90)
- + ( -derive45 * p_sectionCourante->coef_d45)
- + ( -derive90 * p_sectionCourante->coef_d90)
- + ( -integralSum * p_sectionCourante->coef_i90)
+ ((distMurG45Moy / 0.27) * p_sectionCourante->coef_pg45)
+ + ((distMurD45Moy / 0.27) * p_sectionCourante->coef_pd45)
+ + ((distMurD90Moy / 0.17) * p_sectionCourante->coef_pg90)
+ + ((distMurD90Moy / 0.17) * p_sectionCourante->coef_pd90)
+
+
+ //+ ( -integralSum * p_sectionCourante->coef_i90)
)//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
+ DIRECTION_PULSE_MIDDLE;
+
+
+
+
+
+
+
+
+
+/*
+ serialDebug.printf("derive45 => %.4lf \r\n ",derive45);
+ serialDebug.printf("derive90 => %.4lf \r\n ",derive90);
+ serialDebug.printf("distMurG45Moy => %.4lf \r\n ",distMurG45Moy);
+ serialDebug.printf("distMurD45Moy => %.4lf \r\n ",distMurD45Moy);
+ serialDebug.printf("trueDistMurG90Moy => %.4lf \r\n ",trueDistMurG90Moy);
+ serialDebug.printf("trueDistMurD90Moy => %.4lf \r\n ",trueDistMurD90Moy);
+ serialDebug.printf("positionSurPiste90 => %.4lf \r\n ",positionSurPiste90);
+ serialDebug.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
+ serialDebug.printf("largeurPiste90 => %.4lf \r\n ",largeurPiste90);*/
#ifdef OMAR