![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 14:d471faa7d1a2
- Parent:
- 13:af9a59ccf60b
- Child:
- 15:129f205ff030
--- a/stateMachines.cpp Mon Sep 10 21:00:47 2018 +0000 +++ b/stateMachines.cpp Tue Sep 11 09:55:49 2018 +0000 @@ -49,7 +49,7 @@ AnalogIn anaDlvvFront(PA_4);//capteur ir avant #endif -int differenceGD45; +int differenceGD45,differenceGD90; //Capteur vitesse InterruptIn it_tachymeter(PA_11); @@ -97,6 +97,9 @@ return;//on attend encore un peu l'aquisition de la vitesse } tachySectionDist_cm += tachyStepsRegister * 4; +#ifdef DEBUG + pc.printf("IT: distance parcourue %d\r\n",tachySectionDist_cm); +#endif tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us(); tachyStepsRegister=0; timerSinceGetTachy.reset(); @@ -111,16 +114,15 @@ { sumMoy+=tab[k]; } -#if (NB_ECHANTILLONS_IR == 4 && NB_ECHANTILLONS_LIDAR == 4) - return (uint16_t)sumMoy >> 2; -#else return sumMoy/size; -#endif } void it4cm() { tachyStepsRegister+=4; +#ifdef DEBUG + pc.printf("IT tachy\r\n"); +#endif } void it_serial() @@ -169,7 +171,7 @@ pc.printf("Init Section\r\n"); #endif st_currentSection=ARRET; - p_sectionCourante->nextSection=&p_section1; + p_sectionCourante=&p_section1; it_tachymeter.fall(&it4cm); timerSinceGetTachy.start(); getTachySpeed();//to reset @@ -185,7 +187,8 @@ p_section1.ms_decel = 0.0001; p_section1.lidarWarningDist_cm = 200; p_section1.lng_section_cm = 1000;//10m - p_section1.coef_p = 93400000.0; + //p_section1.coef_p = 93400000.0; + p_section1.coef_p = 25000000.0; p_section1.coef_i = 0.0; p_section1.coef_d = 0.0; return; @@ -199,6 +202,7 @@ st_maxSpeed=SPEED_MAX; maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps; serialLidar.baud(115200); + serialLidar.attach(&it_serial); return; } @@ -228,6 +232,9 @@ //########## UPDATE STATES ########## void mursUpdate(void) { +#if (DEBUG > 3) + pc.printf("\r\nUpdate MURS\r\n"); +#endif MUR_ST st_tmpMurs; //lectures distMurG90[index_fifo_ir]=anaG90.read_u16(); @@ -238,6 +245,10 @@ distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR); distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR); + +#if DEBUG > 1 + pc.printf("dist45G:%d dist45D:%d Deadzone: %d\r\n",distMurG45Moy,distMurD45Moy,IR_DEADZONE_U16_22cm); +#endif switch (st_murs) { case EQUILIBRAGE_REPULSIF: @@ -289,6 +300,9 @@ void sectionUpdate(void) { +#if (DEBUG > 3) + pc.printf("\r\nUpdate Section\r\n"); +#endif SECTION_ST st_tmpSection; switch (st_currentSection) { case RUNNING_SECTION: @@ -296,6 +310,10 @@ { st_tmpSection = LOADING_SECTION; } + else + { + return; + } break; case LOADING_SECTION: if(p_sectionCourante->nextSection != NULL) @@ -307,7 +325,7 @@ } break; case ARRET: - if(p_sectionCourante->nextSection != NULL) + if(p_sectionCourante != NULL) { st_tmpSection = RUNNING_SECTION; } @@ -325,6 +343,9 @@ void maxSpeedUpdate(void) { +#if (DEBUG > 3) + pc.printf("\r\nUpdate MaxSpeed\r\n"); +#endif MAX_SPEED_ST st_tmpMaxSpeed; i=0; @@ -352,7 +373,11 @@ void throttleUpdate(void) { +#if (DEBUG > 3) + pc.printf("\r\nUpdate Throttle\r\n"); +#endif THROTTLE_ST st_tmpThro; + getTachySpeed(); switch (st_thro) { case UNDER_SPEED: @@ -397,15 +422,23 @@ //updating output parameters void mursOutput(void) { +#if (DEBUG > 3) + pc.printf("\r\n Output MURS\r\n"); +#endif switch (st_murs) { case EQUILIBRAGE_REPULSIF://TO DO TODO case ATTRACTIF_G: case ATTRACTIF_D: + + differenceGD90 = distMurD90Moy - distMurG90Moy; differenceGD45 = distMurD45Moy - distMurG45Moy; - pulseDirection = (double) ((double)differenceGD45 / (double)p_sectionCourante->coef_p) + 0.0015; + pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015; default: break; } + #if DEBUG > 1 + pc.printf("PWM pulse: %.7f\r\n",pulseDirection); + #endif PwmDirection.pulsewidth(pulseDirection); return; } @@ -418,7 +451,9 @@ #endif void sectionOutput(void) { - +#if (DEBUG > 3) + pc.printf("\r\n Output Section\r\n"); +#endif switch (st_currentSection) { case RUNNING_SECTION: break; @@ -438,6 +473,9 @@ void maxSpeedOutput(void) { +#if (DEBUG > 3) + pc.printf("\r\n Output MAX SPEED\r\n"); +#endif switch(st_maxSpeed) { case SPEED_LIMITED: @@ -455,6 +493,9 @@ void throttleOutput(void) { +#if (DEBUG > 3) + pc.printf("\r\n Output TROTTLE\r\n"); +#endif switch (st_thro) { case UNDER_SPEED: ms_pwmSpeedPulse += p_sectionCourante->ms_accel; @@ -466,9 +507,10 @@ default: break; } -#ifdef DEBUG +#if DEBUG > 0 pc.printf("\r\nodo:%d dist = %d \tstrength = %d \tC45D: %d C45G: %d C90D: %d C90G: %d looptime: %d micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value pc.printf("\r\nstate Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro); + wait(2); timeSinceStart.reset(); timeSinceStart.start(); #endif