Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 29:fc984fe08ca7
- Parent:
- 27:f8c3f1524a64
- Child:
- 30:21642fb8297a
--- a/stateMachines.cpp Mon Sep 17 11:08:02 2018 +0000
+++ b/stateMachines.cpp Tue Sep 18 18:28:19 2018 +0000
@@ -2,31 +2,36 @@
#if DEBUG >0
Serial pc(USBTX, USBRX); // tx, rx
#endif
-#if DEBUG >= -1
-InterruptIn my_button(USER_BUTTON);
+
+//*************** declarations ***************
#ifdef DUMP_SAMPLIG_PERIOD
Timer timerLog;
#endif
-#endif
-uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
-uint16_t distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
-uint16_t distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
-uint16_t distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
+double distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
+double distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
+double shortDistMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche petites distances pour moyenne
+double shortDistMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit petites distances pour moyenne
+double distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
+double distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
#ifdef DLVV
uint16_t distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne
uint16_t distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne
uint16_t distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front
#endif
-uint16_t distMurG90Moy;
-uint16_t distMurD90Moy;
-uint16_t distMurG45Moy;
-uint16_t distMurD45Moy;
+double distMurG90Moy;
+double distMurD90Moy;
+double shortDistMurG90Moy;
+double shortDistMurD90Moy;
+double trueDistMurG90Moy;
+double trueDistMurD90Moy;
+double distMurG45Moy;
+double distMurD45Moy;
#ifdef DLVV
-uint16_t distMurG10Moy;
-uint16_t distMurD10Moy;
-uint16_t distMurFrontMoy
+double distMurG10Moy;
+double distMurD10Moy;
+double distMurFrontMoy
#endif
int index_fifo_ir = 0;//pour géreer le buffer tournant
int index_fifo_lidar = 0;
@@ -41,11 +46,10 @@
int pulseDirection_us = DIRECTION_PULSE_MIDDLE;
int pulseSpeed_us = INITAL_PULSE_SPEED_US;
//Capteurs direction
-
-
-
AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche
AnalogIn anaD90(CAPT_90_DROITE);//capteur ir coté droit
+AnalogIn anaShortG90(CAPT_90_GAUCHE_SHORT);//capteur ir coté gauche short
+AnalogIn anaShortD90(CAPT_90_DROITE_SHORT);//capteur ir coté droit short
AnalogIn anaG45(CAPT_45_GAUCHE);//capteur ir avant gauche 45 deg
AnalogIn anaD45(CAPT_45_DROITE);//capteur ir avant droit 45deg
#ifdef DLVV
@@ -54,7 +58,14 @@
AnalogIn anaDlvvFront(CAPT_DEVANT);//capteur ir avant
#endif
-int differenceGD45,differenceGD90,prevDiffGD90,prevDiffGD45,derive45,derive90;
+//piste
+double largeurPiste = 150.0;
+double positionSurPiste90 = 75.0;
+double positionSurPiste90Prev = positionSurPiste90;
+double positionSurPiste45 = 75.0;
+double positionSurPiste45Prev = positionSurPiste45;
+
+int derive45,derive90;
int lastDifferences90[NB_INTEGRAL_SAMPLES] = {0};//for integral correction
int lastDifferenceIndex = 0;
int integralSum;
@@ -76,10 +87,10 @@
const int HEADER=0x59;// data package frame header
//SPEED
-int maxSpeed_cmps = 0;
-int tachySpeed_cmps = 0;
-int tachyStepsRegister = 0;
-int tachySectionDist_cm = 0;
+double maxSpeed_cmps = 0;
+double tachySpeed_cmps = 0;
+double tachyStepsRegister = 0;
+double tachySectionDist_cm = 0;
#ifdef FREINAGE_ADAPTATIF
Timer brakingTimer;
@@ -119,8 +130,10 @@
history[m].states.maxSpeed = '0';
history[m].states.throttle = '0';
history[m].time = 0;
- history[m].diffgd45 = 0;
- history[m].diffgd90 = 0;
+ history[m].position45 = 0.0;
+ history[m].position90 = 0.0;
+ history[m].largeurPiste = 0.0;
+ history[m].dist = 0.0;
history[m].pwm_thro_us = 0;
history[m].pwm_dir_us = 0;
history[m].distLidar = 0;
@@ -148,12 +161,13 @@
history[indexSample].states.maxSpeed = (char)st_maxSpeed;
history[indexSample].states.throttle = (char)st_thro;
history[indexSample].time = timeSinceStart.read_us() ;
- history[indexSample].diffgd45 = differenceGD45;
- history[indexSample].diffgd90 = differenceGD90;
+ history[indexSample].position45 = positionSurPiste45;
+ history[indexSample].position90 = positionSurPiste90;
+ history[indexSample].largeurPiste = largeurPiste;
history[indexSample].pwm_thro_us = pulseSpeed_us;
history[indexSample].pwm_dir_us = pulseDirection_us;
history[indexSample].dist = tachySectionDist_cm,
- history[indexSample].distLidar = distLidar;
+ history[indexSample].distLidar = distLidar;
history[indexSample].strLidar = strengthLidar;
indexSample++;
#if DEBUG > 0
@@ -174,12 +188,12 @@
#if DEBUG > 0
pc.printf("[START TO TRANSMIT]\r\n");
#endif
- serialLidar.printf("time,diffgd 45,diffgd 90,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
+ serialLidar.printf("time,position 45,position 90,largeur pistepwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
for(int p=0; p<indexSample; p++) {
- serialLidar.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
+ serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d,%d\r\n",
history[p].time,
- history[p].diffgd45,
- history[p].diffgd90,
+ history[p].position45,
+ history[p].position90,
history[p].pwm_thro_us,
history[p].pwm_dir_us,
history[p].dist,
@@ -206,6 +220,15 @@
}
#endif
+void initIntegrationTable()
+{
+ for(int h=0;h<NB_INTEGRAL_SAMPLES;h++)
+ {
+ lastDifferences90[h] = 0;
+ }
+ return;
+}
+
void getTachySpeed()
{
//tachySteps = VALEUR_DU_PIN;
@@ -218,15 +241,31 @@
#if DEBUG > 2
pc.printf("IT: distance parcourue %d , vitesse:%d \r\n",tachySectionDist_cm,tachySpeed_cmps);
#endif
- tachyStepsRegister=0;
+ tachyStepsRegister=0.0;
timerSinceGetTachy.reset();
timerSinceGetTachy.start();
return;
}
-uint16_t getDistMoy(uint16_t* tab,int size)
+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);
+
+ int sumMoy = 0;
+ for(int k=0; k<size; k++) {
+ sumMoy+=tab[k];
+ }
+ return sumMoy/size;
+}
+
+double getDistMoy(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
+ tab[index_fifo_ir] = 1.0/(0.0161 * tab[index_fifo_ir]);
int sumMoy = 0;
for(int k=0; k<size; k++) {
sumMoy+=tab[k];
@@ -236,7 +275,7 @@
void it4cm()
{
- tachyStepsRegister+=TACHY_CM;
+ tachyStepsRegister += TACHY_CM;
#if DEBUG > 0
pc.printf("IT tachy\r\n");
#endif
@@ -266,20 +305,14 @@
//########## INIT STATES MACHINES ##########
void mursInit(void)
{
-#if DEBUG >= -1
- my_button.fall(&pressed);
- initSamples();
-#endif
#if DEBUG > 0
pc.baud(115200);
pc.printf("Init Murs\r\n");
#endif
timeSinceStart.start();
- st_murs=EQUILIBRAGE_REPULSIF;
+ st_murs=REF_BIDIR;
PwmDirection.period_us(SPEED_PERIOD_US);
PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu
- prevDiffGD45 = 0;
- prevDiffGD90 = 0;
return;
}
#ifdef DLVV
@@ -288,7 +321,7 @@
#if DEBUG > 0
pc.printf("Init Obstacle\r\n");
#endif
- st_speedLimit=FRONT_CLEAR;
+ st_speedLimit=ALL_CLEAR;
return;
}
#endif
@@ -307,27 +340,27 @@
//section de test 1
p_section1.nextSection =NULL;// &p_section2;
- p_section1.targetSpeed_cmps = 400;
- p_section1.slowSpeed_cmps = 328;
- p_section1.brakingCoefficient = 61; // application de la formule
+ p_section1.consigne_position = 75.0;
+ p_section1.targetSpeed_cmps = 400.0;
+ p_section1.slowSpeed_cmps = 328.0;
p_section1.coef_p_speed = 1;
- p_section1.lidarWarningDist_cm = 120;
- p_section1.lng_section_cm = 30000;//30m
- p_section1.coef_p = 50;//35;
- p_section1.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES;
- p_section1.coef_d = 10000;//35;
+ p_section1.lidarWarningDist_cm = 120.0;
+ p_section1.lng_section_cm = 30000.0;//30m
+ p_section1.coef_p = 0.02;
+ p_section1.coef_i = 0.00001;
+ p_section1.coef_d = 0.00001;
//section de test
p_section2.nextSection = NULL;
- p_section2.targetSpeed_cmps = 328;
- p_section2.slowSpeed_cmps = 328;
- p_section2.brakingCoefficient = 0; // application de la formule
- p_section2.coef_p_speed = 1;
- p_section2.lidarWarningDist_cm = 300;
- p_section2.lng_section_cm = 200;//2m
- p_section2.coef_p = 50;
- p_section2.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES;
- p_section2.coef_d = 10000;//35;
+ 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 = 0.02;
+ p_section2.coef_i = 0.00001;
+ p_section2.coef_d = 0.00001;
return;
}
@@ -379,16 +412,48 @@
pc.printf("\r\nUpdate MURS\r\n");
#endif
//lectures
- distMurG90[index_fifo_ir]=anaG90.read_u16();
- distMurD90[index_fifo_ir]=anaD90.read_u16();
- distMurG45[index_fifo_ir]=anaG45.read_u16();
- distMurD45[index_fifo_ir]=anaD45.read_u16();
+
+ distMurG45Moy = getDistMoy(&anaG45,distMurG45,NB_ECHANTILLONS_IR);
+ distMurD45Moy = getDistMoy(&anaD45,distMurD45,NB_ECHANTILLONS_IR);
+ distMurG90Moy = getDistMoy(&anaG90,distMurG90,NB_ECHANTILLONS_IR);
+ distMurD90Moy = getDistMoy(&anaG90,distMurD90,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;
-
- distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR);
- distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR);
- distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
- distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
+
+ if(shortDistMurG90Moy < DIST_MIN_LONG_CM)
+ {
+ trueDistMurG90Moy = shortDistMurG90Moy;
+ }else
+ {
+ trueDistMurG90Moy = distMurG90Moy;
+ }
+ if(shortDistMurD90Moy < DIST_MIN_LONG_CM)
+ {
+ trueDistMurD90Moy = shortDistMurD90Moy;
+ }else
+ {
+ trueDistMurD90Moy = distMurD90Moy;
+ }
+
+#ifdef DLVV
+ switch (st_obstacle) {
+ case FRONT_OBSTRUCTED:
+ st_murs = REF_A_GAUCHE;
+ return;
+ case RIGHT_OBSTRUCTED:
+ st_murs = REF_A_GAUCHE;
+ return;
+ case LEFT_OBSTRUCTED:
+ st_murs = REF_A_DROITE;
+ return;
+ default:
+ break;
+ }
+
+#endif
+ st_murs = REF_BIDIR;
+
return;
}
@@ -443,7 +508,7 @@
if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) {
if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
st_tmpMaxSpeed = SPEED_MAX;
- } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD &&
+ } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD &&
strengthLidarPrev > LIDAR_STRENGTH_THRESOLD &&
distLidar < p_sectionCourante->lidarWarningDist_cm &&
distLidarPrev-distLidar > 3)
@@ -470,15 +535,15 @@
switch (st_thro) {
case REGULATION_SPEED:
if( st_currentSection == ARRET ||
- st_maxSpeed == BLOCKED )
+ st_maxSpeed == BLOCKED )
{
st_tmpThro = BRAKING;
- }
+ }
#ifdef FREINAGE_ADAPTATIF
else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS))
{
st_tmpThro = BRAKING;
- brakingDurationNeeded_us = (10000)* MASSE_BINIOU_KG *((tachySpeed_cmps-maxSpeed_cmps)*(tachySpeed_cmps+maxSpeed_cmps)) / (PUISSANCE_FREINAGE_W *2) ;//vitesse en m/s et temps en us
+ brakingDurationNeeded_us = 10000.0 * MASSE_BINIOU_KG *((tachySpeed_cmps-maxSpeed_cmps)*(tachySpeed_cmps+maxSpeed_cmps)) / (PUISSANCE_FREINAGE_W *2) ;//vitesse en m/s et temps en us
brakingTimer.reset();
brakingTimer.start();
}
@@ -489,7 +554,7 @@
}
break;
case BRAKING:
- if( BLOCKED
+ if( st_maxSpeed == BLOCKED
#ifdef FREINAGE_ADAPTATIF
|| brakingTimer.read_us() < brakingDurationNeeded_us
#endif
@@ -524,21 +589,37 @@
//updating output parameters
void mursOutput(void)
{
- prevDiffGD90 = differenceGD90;
- prevDiffGD45 = differenceGD45;
+ //pour dériver
+ positionSurPiste45Prev = positionSurPiste45;
+ positionSurPiste90Prev = positionSurPiste90;
#if (DEBUG > 3)
pc.printf("\r\n Output MURS\r\n");
#endif
- differenceGD90 = distMurD90Moy - distMurG90Moy;
- differenceGD45 = distMurD45Moy - distMurG45Moy;
+ switch (st_murs) {
+ case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche
+ largeurPiste = 150.0;
+ positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM );
+ positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
+ break;
+ case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche
+ largeurPiste = 150.0;
+ positionSurPiste90 = largeurPiste - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
+ positionSurPiste45 = largeurPiste - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM;
+ break;
+ default://REF_BIDIR
+ largeurPiste = trueDistMurG90Moy + trueDistMurD90Moy + (2 * DEMI_LARGEUR_BINIOU_CM);
+ positionSurPiste90 = (trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM);
+ positionSurPiste45 = (largeurPiste / 2) + ((distMurG45Moy - distMurD45Moy) * (1.414214)/2 );
+ break;
+ }
//deriv correction
- derive45 = differenceGD45 - prevDiffGD45;
- derive90 = differenceGD90 - prevDiffGD90;
+ derive45 = positionSurPiste45 - positionSurPiste45Prev;
+ derive90 = positionSurPiste90 - positionSurPiste90Prev;
//integral correction
- lastDifferences90[lastDifferenceIndex] = differenceGD90;
+ lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
integralSum=0;
for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
integralSum+=lastDifferences90[f];
@@ -546,9 +627,12 @@
lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
//application des coefficients
- pulseDirection_us = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p)
- + ( derive45 / p_sectionCourante->coef_d)
- + (integralSum / p_sectionCourante->coef_i)
+ pulseDirection_us = 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)
+ + ( derive45 * p_sectionCourante->coef_d)
+ + (integralSum * p_sectionCourante->coef_i)
+ )
+ DIRECTION_PULSE_MIDDLE;
//gestioon du dépassement