![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 15:129f205ff030
- Parent:
- 14:d471faa7d1a2
- Child:
- 16:63690703b5b6
--- a/stateMachines.cpp Tue Sep 11 09:55:49 2018 +0000 +++ b/stateMachines.cpp Tue Sep 11 12:28:37 2018 +0000 @@ -19,9 +19,6 @@ uint16_t distMurG10Moy; uint16_t distMurD10Moy; uint16_t distMurFrontMoy - - - #endif int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant int index_fifo_ir = 0;//pour géreer le buffer tournant @@ -29,12 +26,12 @@ //sections s_Section p_section1; -//Controls +//PWM Controls PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4 PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3 -float pulseDirection = 0.0015; -float pulseSpeed = 0.015; +double pulseDirection = 0.0015; +double pulseSpeed = 0.015; //Capteurs direction @@ -70,7 +67,6 @@ int tachySpeed_cmps = 0; int tachyStepsRegister = 0; int tachySectionDist_cm = 0; -float ms_pwmSpeedPulse = 0.0015;//IDLE //Etats MUR_ST st_murs; @@ -96,11 +92,11 @@ { 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(); + tachySectionDist_cm += tachyStepsRegister; + tachySpeed_cmps = (tachyStepsRegister * 1000000)/timerSinceGetTachy.read_us(); + #if DEBUG > 2 + pc.printf("IT: distance parcourue %d , vitesse:%d \r\n",tachySectionDist_cm,tachySpeed_cmps); + #endif tachyStepsRegister=0; timerSinceGetTachy.reset(); timerSinceGetTachy.start(); @@ -183,12 +179,12 @@ p_section1.targetSpeed_cmps = 50; p_section1.slowSpeed_cmps = 15; p_section1.brakingCoefficient = 20; // application de la formule - p_section1.ms_accel = 0.00005; - p_section1.ms_decel = 0.0001; + p_section1.ms_accel = 0.000004; + p_section1.ms_decel = 0.0000001; p_section1.lidarWarningDist_cm = 200; - p_section1.lng_section_cm = 1000;//10m + p_section1.lng_section_cm = 500;//500cm //p_section1.coef_p = 93400000.0; - p_section1.coef_p = 25000000.0; + p_section1.coef_p = 35000000.0; p_section1.coef_i = 0.0; p_section1.coef_d = 0.0; return; @@ -219,6 +215,7 @@ wait(1); PwmMotor.pulsewidth(0.0015); wait(1); + pulseSpeed = 0.0015; #ifdef DEBUG pc.printf("temps init: %d micros\r\n",timeSinceStart.read_us()); pc.printf("\r\nStates INIT: state Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro); @@ -433,11 +430,25 @@ differenceGD90 = distMurD90Moy - distMurG90Moy; differenceGD45 = distMurD45Moy - distMurG45Moy; pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015; + if(pulseDirection > 0.002) + { +#if DEBUG > 1 + pc.printf("!!! OVER PWM Direction pulse: %.7f\r\n",pulseDirection); +#endif + pulseDirection = 0.002; + } + else if(pulseDirection < 0.001) + { +#if DEBUG > 1 + pc.printf("!!! UNDER PWM Direction pulse: %.7f\r\n",pulseDirection); +#endif + pulseDirection = 0.001; + } default: break; } #if DEBUG > 1 - pc.printf("PWM pulse: %.7f\r\n",pulseDirection); + pc.printf("PWM Direction pulse: %.7f\r\n",pulseDirection); #endif PwmDirection.pulsewidth(pulseDirection); return; @@ -462,6 +473,7 @@ tachySectionDist_cm = 0; break; case ARRET: + PwmMotor.pulsewidth(0.00135); wait(10);//on est à l'arret break; default: @@ -498,19 +510,29 @@ #endif switch (st_thro) { case UNDER_SPEED: - ms_pwmSpeedPulse += p_sectionCourante->ms_accel; + if ( (pulseSpeed + p_sectionCourante->ms_decel) < 0.002) + { + pulseSpeed += p_sectionCourante->ms_accel; + } break; case OVER_SPEED: - ms_pwmSpeedPulse -= p_sectionCourante->ms_decel; + if ( (pulseSpeed - p_sectionCourante->ms_decel) > 0.0015) + { + pulseSpeed -= p_sectionCourante->ms_decel; + } break; case AT_SPEED: default: break; } + PwmMotor.pulsewidth(pulseSpeed); +#if DEBUG > 1 + pc.printf("PWM Thro pulse: %.7f\r\n",pulseSpeed); +#endif #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); + //wait(2); timeSinceStart.reset(); timeSinceStart.start(); #endif