Hlimi Omar
/
biniou
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 17:8c465656eea4
- Parent:
- 16:63690703b5b6
- Child:
- 18:38504337d2fc
--- a/stateMachines.cpp Tue Sep 11 14:49:55 2018 +0000 +++ b/stateMachines.cpp Wed Sep 12 22:14:59 2018 +0000 @@ -30,8 +30,8 @@ PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4 PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3 -double pulseDirection = 0.0015; -double pulseSpeed = INITAL_PULSE_SPEED; +int pulseDirection = DIRECTION_PULSE_MIDDLE; +int pulseSpeed = INITAL_PULSE_SPEED; //Capteurs direction @@ -147,8 +147,8 @@ #endif timeSinceStart.start(); st_murs=EQUILIBRAGE_REPULSIF; - PwmDirection.period(0.010); //20 ms is default - PwmDirection.pulsewidth(0.0015); + PwmDirection.period_us(SPEED_PERIOD); + PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu return; } #ifdef DLVV @@ -176,17 +176,16 @@ //section de test p_section1.nextSection = NULL; - p_section1.targetSpeed_cmps = 100; - p_section1.slowSpeed_cmps = 15; + p_section1.targetSpeed_cmps = 0; + p_section1.slowSpeed_cmps = 0; p_section1.brakingCoefficient = 20; // application de la formule - p_section1.ms_accel = 0.0000003; - p_section1.ms_decel = 0.00000015; - p_section1.lidarWarningDist_cm = 200; - p_section1.lng_section_cm = 1000;//1000cm - //p_section1.coef_p = 93400000.0; - p_section1.coef_p = 35000000.0; - p_section1.coef_i = 0.0; - p_section1.coef_d = 0.0; + p_section1.us_accel = 1;//+1 microsec each loop + p_section1.us_decel = 1; + p_section1.lidarWarningDist_cm = 100; + p_section1.lng_section_cm = 10000;//1500;//1500cm + p_section1.coef_p = 35; + p_section1.coef_i = 0; + p_section1.coef_d = 0; return; } @@ -208,12 +207,12 @@ pc.printf("Init Throttle\r\n"); #endif st_thro = UNDER_SPEED; - PwmMotor.period(0.020); //20 ms is default - PwmMotor.pulsewidth(0.0010); + PwmMotor.period_us(SPEED_PERIOD); //20 ms is default + PwmMotor.pulsewidth_us(1000); wait(3); - PwmMotor.pulsewidth(0.0020); + PwmMotor.pulsewidth_us(2000); wait(1); - PwmMotor.pulsewidth(0.0015); + PwmMotor.pulsewidth_us(1500); wait(1); pulseSpeed = INITAL_PULSE_SPEED; #ifdef DEBUG @@ -381,7 +380,7 @@ { st_tmpThro = BRAKING; } - else if( tachySpeed_cmps > maxSpeed_cmps ) + else if( tachySpeed_cmps >= maxSpeed_cmps ) { st_tmpThro = AT_SPEED; }else @@ -394,7 +393,7 @@ { st_tmpThro = BRAKING; } - else if( tachySpeed_cmps < maxSpeed_cmps) + else if( tachySpeed_cmps <= maxSpeed_cmps) { st_tmpThro = AT_SPEED; }else @@ -458,28 +457,28 @@ differenceGD90 = distMurD90Moy - distMurG90Moy; differenceGD45 = distMurD45Moy - distMurG45Moy; - pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015; - if(pulseDirection > MAX_PULSE_WIDTH_FOR_TACHY) + pulseDirection = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p) + 1500; + if(pulseDirection > DIRECTION_PULSE_MAX)//POUR TOURNER A GAUCHE { #if DEBUG > 1 - pc.printf("!!! OVER PWM Direction pulse: %.7f\r\n",pulseDirection); + pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection); #endif - pulseDirection = MAX_PULSE_WIDTH_FOR_TACHY; + pulseDirection = DIRECTION_PULSE_MAX; } - else if(pulseDirection < 0.001) + else if(pulseDirection < DIRECTION_PULSE_MIN )//POUR TOURNER A DROITE { #if DEBUG > 1 - pc.printf("!!! UNDER PWM Direction pulse: %.7f\r\n",pulseDirection); + pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection); #endif - pulseDirection = 0.001; + pulseDirection = DIRECTION_PULSE_MIN ; } default: break; } #if DEBUG > 1 - pc.printf("PWM Direction pulse: %.7f\r\n",pulseDirection); + pc.printf("PWM Direction pulse: %d\r\n",pulseDirection); #endif - PwmDirection.pulsewidth(pulseDirection); + PwmDirection.pulsewidth_us(pulseDirection); return; } @@ -538,15 +537,15 @@ #endif switch (st_thro) { case UNDER_SPEED: - if ( (pulseSpeed + p_sectionCourante->ms_decel) < 0.002) + if ( (pulseSpeed + p_sectionCourante->us_accel) < MAX_PULSE_WIDTH_FOR_TACHY) { - pulseSpeed += p_sectionCourante->ms_accel; + pulseSpeed += p_sectionCourante->us_accel; } break; case OVER_SPEED: - if ( (pulseSpeed - p_sectionCourante->ms_decel) > 0.0015) + if ( (pulseSpeed - p_sectionCourante->us_decel) > INITAL_PULSE_SPEED) { - pulseSpeed -= p_sectionCourante->ms_decel; + pulseSpeed -= p_sectionCourante->us_decel; } break; case AT_SPEED: @@ -560,9 +559,9 @@ default: break; } - PwmMotor.pulsewidth(pulseSpeed); + PwmMotor.pulsewidth_us(pulseSpeed); #if DEBUG > 1 - pc.printf("PWM Thro pulse: %.7f\r\n",pulseSpeed); + pc.printf("PWM Thro pulse: %d\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