Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 19:771bf61be276
- Parent:
- 18:38504337d2fc
- Child:
- 20:017ec2428e06
--- a/stateMachines.cpp Wed Sep 12 22:37:07 2018 +0000
+++ b/stateMachines.cpp Thu Sep 13 23:07:15 2018 +0000
@@ -1,7 +1,11 @@
#include "stateMachines.h"
-#ifdef DEBUG
+#if DEBUG >0
Serial pc(USBTX, USBRX); // tx, rx
#endif
+#if DEBUG >= -1
+InterruptIn my_button(USER_BUTTON);
+#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
@@ -31,9 +35,7 @@
PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3
int pulseDirection = DIRECTION_PULSE_MIDDLE;
-int pulseSpeed = INITAL_PULSE_SPEED;
-
-
+int pulseSpeed_us = INITAL_PULSE_SPEED_US;
//Capteurs direction
AnalogIn anaG90(PC_1);//capteur ir coté gauche
@@ -46,7 +48,7 @@
AnalogIn anaDlvvFront(PA_4);//capteur ir avant
#endif
-int differenceGD45,differenceGD90;
+int differenceGD45,differenceGD90,prevDiffGD90,prevDiffGD45,derive45,derive90;
//Capteur vitesse
InterruptIn it_tachymeter(PA_11);
@@ -84,19 +86,115 @@
Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
Timer timerSinceGetTachy;
+
+// +++++++++++++++++++++++++++++++++++++++++++ SAMPLING +++++++++++++++++++++++++++++++++++++++++++
+#ifdef SAMPLING
+s_Sample history[TAILLE_SAMPLES];
+int indexSample = 0;
+void initSamples(void)
+{
+ for(int m=0; m<TAILLE_SAMPLES; m++) {
+ history[m].states.murs_dlvv = '0';
+ history[m].states.section = '0';
+ history[m].states.maxSpeed = '0';
+ history[m].states.throttle = '0';
+ history[m].time = 0;
+ history[m].cg45 = 0;
+ history[m].cd45 = 0;
+ history[m].cg90 = 0;
+ history[m].cd90 = 0;
+ history[m].lidarDist = 0;
+ history[m].lidarStr = 0;
+ history[m].speed = 0;
+ }
+ #if DEBUG > 0
+ pc.printf("[INIT SAMPLE DONE]\r\n");
+ #endif
+ return;
+}
+void sampleLog(void)
+{
+ if(indexSample < TAILLE_SAMPLES) {
+#ifdef DLVV
+ history[indexSample].states.murs_dlvv = (char)st_obstacle;
+#else
+ history[indexSample].states.murs_dlvv = (char)st_murs;
+#endif
+ history[indexSample].states.section = (char)st_currentSection;
+ history[indexSample].states.maxSpeed = (char)st_maxSpeed;
+ history[indexSample].states.throttle = (char)st_thro;
+ history[indexSample].time = timeSinceStart.read_us() ;
+ history[indexSample].cg45 = distMurG45Moy;
+ history[indexSample].cd45 = distMurD45Moy;
+ history[indexSample].cg90 = distMurG90Moy;
+ history[indexSample].cd90 = distMurD90Moy;
+ history[indexSample].lidarDist = distLidar;
+ history[indexSample].lidarStr = strengthLidar;
+ history[indexSample].speed = tachySpeed_cmps;
+ history[indexSample].dist = tachySectionDist_cm,
+ indexSample++;
+#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
+ }
+ return;
+}
+void transmitData(void)
+{
+ #if DEBUG > 0
+ pc.printf("[START TO TRANSMIT]\r\n");
+ #endif
+ serialLidar.printf("time,gauche 45,droite 45,gauche 90, droite 90,dist,lidar dist,strength,speed,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,%d,%d\r\n",
+ history[p].time,
+ history[p].cg45,
+ history[p].cd45,
+ history[p].cg90,
+ history[p].cd90,
+ history[p].dist,
+ history[p].lidarDist,
+ history[p].lidarStr,
+ history[p].speed,
+ history[p].states.murs_dlvv,
+ history[p].states.section,
+ history[p].states.maxSpeed,
+ history[p].states.throttle);
+ }
+ return;
+}
+
+#endif
+
+// +++++++++++++++++++++++++++++++++++++++++++ FUNCTION UTILS +++++++++++++++++++++++++++++++++++++++++++
+
+#if DEBUG >= -1
+void pressed(void)
+{
+ #if DEBUG > 0
+ pc.printf("[BTN PRESSED]\r\n");
+ #endif
+ //p_sectionCourante = &p_section1;
+ transmitData();
+}
+#endif
+
void getTachySpeed()
{
//tachySteps = VALEUR_DU_PIN;
//poour gérer les vitesses lentes
- if(tachyStepsRegister == 0 && timerSinceGetTachy.read_us() < 500000)
- {
+ if(tachyStepsRegister == 0 && timerSinceGetTachy.read_us() < 500000) {
return;//on attend encore un peu l'aquisition de la vitesse
}
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
+#if DEBUG > 2
+ pc.printf("IT: distance parcourue %d , vitesse:%d \r\n",tachySectionDist_cm,tachySpeed_cmps);
+#endif
tachyStepsRegister=0;
timerSinceGetTachy.reset();
timerSinceGetTachy.start();
@@ -106,8 +204,7 @@
uint16_t getDistMoy(uint16_t* tab,int size)
{
int sumMoy = 0;
- for(int k=0; k<size;k++)
- {
+ for(int k=0; k<size; k++) {
sumMoy+=tab[k];
}
return sumMoy/size;
@@ -116,7 +213,7 @@
void it4cm()
{
tachyStepsRegister+=4;
-#ifdef DEBUG
+#if DEBUG > 0
pc.printf("IT tachy\r\n");
#endif
}
@@ -138,23 +235,31 @@
}
}
}
+// +++++++++++++++++++++++++++++++++++++++++++ STATES MACHINES +++++++++++++++++++++++++++++++++++++++++++
+
//########## INIT STATES MACHINES ##########
void mursInit(void)
{
-#ifdef DEBUG
+#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;
- PwmDirection.period_us(SPEED_PERIOD);
+ PwmDirection.period_us(SPEED_PERIOD_US);
PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu
+ prevDiffGD45 = 0;
+ prevDiffGD90 = 0;
return;
}
#ifdef DLVV
void obstacleInit(void)
{
-#ifdef DEBUG
+#if DEBUG > 0
pc.printf("Init Obstacle\r\n");
#endif
st_speedLimit=FRONT_CLEAR;
@@ -163,7 +268,7 @@
#endif
void sectionInit(void)
{
-#ifdef DEBUG
+#if DEBUG > 0
pc.printf("Init Section\r\n");
#endif
st_currentSection=ARRET;
@@ -176,22 +281,21 @@
//section de test
p_section1.nextSection = NULL;
- p_section1.targetSpeed_cmps = 10;
+ p_section1.targetSpeed_cmps = 300;
p_section1.slowSpeed_cmps = 5;
- p_section1.brakingCoefficient = 30; // application de la formule
- p_section1.us_accel = 1;//+1 microsec each loop
- p_section1.us_decel = 1;
+ p_section1.brakingCoefficient = 570; // application de la formule
+ p_section1.coef_p_accel = 1;
p_section1.lidarWarningDist_cm = 300;
- p_section1.lng_section_cm = 1500;//1500cm
+ p_section1.lng_section_cm = 1600;//1600cm
p_section1.coef_p = 35;
p_section1.coef_i = 0;
- p_section1.coef_d = 0;
+ p_section1.coef_d = 20;
return;
}
void maxSpeedInit(void)
{
-#ifdef DEBUG
+#if DEBUG > 0
pc.printf("Init Max Speed\r\n");
#endif
st_maxSpeed=SPEED_MAX;
@@ -203,19 +307,19 @@
void throttleInit(void)
{
-#ifdef DEBUG
+#if DEBUG > 0
pc.printf("Init Throttle\r\n");
#endif
st_thro = UNDER_SPEED;
- PwmMotor.period_us(SPEED_PERIOD); //20 ms is default
- PwmMotor.pulsewidth_us(1000);
+ PwmMotor.period_us(DIERCTION_PERIOD_MS); //20 ms is default
+ PwmMotor.pulsewidth_us(1000);//MIN
wait(3);
- PwmMotor.pulsewidth_us(2000);
+ PwmMotor.pulsewidth_us(2000);//MAX
wait(1);
- PwmMotor.pulsewidth_us(1500);
+ PwmMotor.pulsewidth_us(1500);//ZEROING
wait(1);
- pulseSpeed = INITAL_PULSE_SPEED;
-#ifdef DEBUG
+ pulseSpeed_us = INITAL_PULSE_SPEED_US;
+#if DEBUG > 0
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);
timeSinceStart.reset();
@@ -241,48 +345,41 @@
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:
- distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
- distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
- if( distMurG90Moy <= IR_DEADZONE_U16_22cm)
- {
- st_tmpMurs = ATTRACTIF_D;
- }else if(distMurD90Moy <= IR_DEADZONE_U16_22cm)
- {
- st_tmpMurs = ATTRACTIF_G;
- }else
- {
- st_tmpMurs = EQUILIBRAGE_REPULSIF;
- }
- break;
- case ATTRACTIF_D:
- distMurD90Moy= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
- if( distMurD90Moy >= IR_FAR_U16_105cm)
- {
- st_tmpMurs = ATTRACTIF_D;
- }else
- {
- st_tmpMurs = EQUILIBRAGE_REPULSIF;
- }
- break;
- case ATTRACTIF_G:
- distMurG90Moy= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
- if(distMurG90Moy >= IR_FAR_U16_105cm)
- {
- st_tmpMurs = ATTRACTIF_G;
- }else
- {
- st_tmpMurs = EQUILIBRAGE_REPULSIF;
- }
- break;
- default:
- return;
+ case EQUILIBRAGE_REPULSIF:
+ distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
+ distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
+ if( distMurG90Moy <= IR_DEADZONE_U16_22cm) {
+ st_tmpMurs = ATTRACTIF_D;
+ } else if(distMurD90Moy <= IR_DEADZONE_U16_22cm) {
+ st_tmpMurs = ATTRACTIF_G;
+ } else {
+ st_tmpMurs = EQUILIBRAGE_REPULSIF;
+ }
+ break;
+ case ATTRACTIF_D:
+ distMurD90Moy= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
+ if( distMurD90Moy >= IR_FAR_U16_105cm) {
+ st_tmpMurs = ATTRACTIF_D;
+ } else {
+ st_tmpMurs = EQUILIBRAGE_REPULSIF;
+ }
+ break;
+ case ATTRACTIF_G:
+ distMurG90Moy= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
+ if(distMurG90Moy >= IR_FAR_U16_105cm) {
+ st_tmpMurs = ATTRACTIF_G;
+ } else {
+ st_tmpMurs = EQUILIBRAGE_REPULSIF;
+ }
+ break;
+ default:
+ return;
}
st_murs = st_tmpMurs;
return;
@@ -301,37 +398,29 @@
#endif
SECTION_ST st_tmpSection;
switch (st_currentSection) {
- case RUNNING_SECTION:
- if(tachySectionDist_cm > p_sectionCourante->lng_section_cm)//on pourrait rajouter un test lidar
- {
- st_tmpSection = LOADING_SECTION;
- }
- else
- {
- return;
- }
- break;
- case LOADING_SECTION:
- if(p_sectionCourante != NULL) //la section a ete chargee dans sectionOutput
- {
- st_tmpSection = RUNNING_SECTION;
- }else
- {
- st_tmpSection=ARRET;
- }
- break;
- case ARRET:
- if(p_sectionCourante != NULL)
- {
- st_tmpSection = RUNNING_SECTION;
- }
- else
- {
- return;
- }
- break;
- default:
- break;
+ case RUNNING_SECTION:
+ if(tachySectionDist_cm > p_sectionCourante->lng_section_cm) { //on pourrait rajouter un test lidar
+ st_tmpSection = LOADING_SECTION;
+ } else {
+ return;
+ }
+ break;
+ case LOADING_SECTION:
+ if(p_sectionCourante != NULL) { //la section a ete chargee dans sectionOutput
+ st_tmpSection = RUNNING_SECTION;
+ } else {
+ st_tmpSection=ARRET;
+ }
+ break;
+ case ARRET:
+ if(p_sectionCourante != NULL) {
+ st_tmpSection = RUNNING_SECTION;
+ } else {
+ return;
+ }
+ break;
+ default:
+ break;
}
st_currentSection = st_tmpSection;
return;
@@ -345,24 +434,20 @@
MAX_SPEED_ST st_tmpMaxSpeed;
i=0;
- if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) )
- {
- if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm)
- {
+ if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) ) {
+ if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm) {
st_tmpMaxSpeed = SPEED_VARIABLE;
- }else
- {
+ } else if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
+ st_tmpMaxSpeed = SPEED_MAX;
+ } else {
st_tmpMaxSpeed = SPEED_LIMITED;
}
- }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA))
- {
+ } else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA)) {
st_tmpMaxSpeed = SPEED_MAX;
- }
- else{
+ } else {
return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil
}
-
st_maxSpeed = st_tmpMaxSpeed;
return;
}
@@ -375,70 +460,54 @@
THROTTLE_ST st_tmpThro;
getTachySpeed();
switch (st_thro) {
- case UNDER_SPEED:
- if(st_currentSection == ARRET)
- {
- st_tmpThro = BRAKING;
- }
- else if( tachySpeed_cmps >= maxSpeed_cmps )
- {
- st_tmpThro = AT_SPEED;
- }else
- {
- return;
- }
- break;
- case OVER_SPEED:
- if(st_currentSection == ARRET)
- {
- st_tmpThro = BRAKING;
- }
- else if( tachySpeed_cmps <= maxSpeed_cmps)
- {
- st_tmpThro = AT_SPEED;
- }else
- {
- return;
- }
- break;
- case AT_SPEED:
- if(st_currentSection == ARRET)
- {
- st_tmpThro = BRAKING;
- }
- else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
- {
- st_tmpThro = OVER_SPEED;
- } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) )
- {
- st_tmpThro = UNDER_SPEED;
- }
- else
- {
- return;
- }
- break;
+ case UNDER_SPEED:
+ if(st_currentSection == ARRET) {
+ st_tmpThro = BRAKING;
+ } else if( tachySpeed_cmps >= maxSpeed_cmps ) {
+ st_tmpThro = AT_SPEED;
+ } else {
+ return;
+ }
+ break;
+ case OVER_SPEED:
+ if(st_currentSection == ARRET) {
+ st_tmpThro = BRAKING;
+ } else if( tachySpeed_cmps <= maxSpeed_cmps) {
+ st_tmpThro = AT_SPEED;
+ } else {
+ return;
+ }
+ break;
+ case AT_SPEED:
+ if(st_currentSection == ARRET) {
+ st_tmpThro = BRAKING;
+ } else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) ) {
+ st_tmpThro = OVER_SPEED;
+ } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) ) {
+ st_tmpThro = UNDER_SPEED;
+ } else {
+ return;
+ }
+ break;
case BRAKING:
- if(st_currentSection == RUNNING_SECTION){
- if( tachySpeed_cmps < maxSpeed_cmps )
- {
- st_tmpThro = UNDER_SPEED;
- }else if( tachySpeed_cmps > maxSpeed_cmps )
- {
- st_tmpThro = OVER_SPEED;
- }else
- {
- st_tmpThro = AT_SPEED;
+ st_tmpThro = STOPPED;
+ break;
+ case STOPPED:
+ if(st_currentSection == RUNNING_SECTION) {
+ if( tachySpeed_cmps < maxSpeed_cmps ) {
+ st_tmpThro = UNDER_SPEED;
+ } else if( tachySpeed_cmps > maxSpeed_cmps ) {
+ st_tmpThro = OVER_SPEED;
+ } else {
+ st_tmpThro = AT_SPEED;
+ }
+ } else {
+ st_tmpThro = STOPPED;
}
- }
- else
- {
- return;
- }
- default:
- break;
+ break;
+ default:
+ break;
}
-
st_thro = st_tmpThro;
return;
}
@@ -451,33 +520,38 @@
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 = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p) + 1500;
- if(pulseDirection > DIRECTION_PULSE_MAX)//POUR TOURNER A GAUCHE
- {
+ case EQUILIBRAGE_REPULSIF://TO DO TODO
+ case ATTRACTIF_G:
+ case ATTRACTIF_D:
+
+ prevDiffGD90 = differenceGD90;
+ prevDiffGD45 = differenceGD45;
+ differenceGD90 = distMurD90Moy - distMurG90Moy;
+ differenceGD45 = distMurD45Moy - distMurG45Moy;
+
+ derive45 = differenceGD45 - prevDiffGD45;
+ derive90 = differenceGD90 - prevDiffGD90;
+
+ pulseDirection = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p)
+ + ( ((derive90*2+derive90*4)/8) / p_sectionCourante->coef_d)
+ + 1500;
+ if(pulseDirection > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
#if DEBUG > 1
- pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection);
+ pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection);
#endif
- pulseDirection = DIRECTION_PULSE_MAX;
- }
- else if(pulseDirection < DIRECTION_PULSE_MIN )//POUR TOURNER A DROITE
- {
+ pulseDirection = DIRECTION_PULSE_MAX;
+ } else if(pulseDirection < DIRECTION_PULSE_MIN ) { //POUR TOURNER A DROITE
#if DEBUG > 1
- pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection);
+ pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection);
#endif
- pulseDirection = DIRECTION_PULSE_MIN ;
- }
- default:
- break;
+ pulseDirection = DIRECTION_PULSE_MIN ;
+ }
+ default:
+ break;
}
- #if DEBUG > 1
+#if DEBUG > 1
pc.printf("PWM Direction pulse: %d\r\n",pulseDirection);
- #endif
+#endif
PwmDirection.pulsewidth_us(pulseDirection);
return;
}
@@ -494,17 +568,17 @@
pc.printf("\r\n Output Section\r\n");
#endif
switch (st_currentSection) {
- case RUNNING_SECTION:
- break;
- case LOADING_SECTION:
- p_sectionCourante=p_sectionCourante->nextSection;
- tachySectionDist_cm = 0;
- break;
- case ARRET:
- //on est à l'arret
- break;
- default:
- break;
+ case RUNNING_SECTION:
+ break;
+ case LOADING_SECTION:
+ p_sectionCourante=p_sectionCourante->nextSection;
+ tachySectionDist_cm = 0;
+ break;
+ case ARRET:
+ //on est à l'arret
+ break;
+ default:
+ break;
}
return;
}
@@ -515,17 +589,16 @@
#if (DEBUG > 3)
pc.printf("\r\n Output MAX SPEED\r\n");
#endif
- switch(st_maxSpeed)
- {
- case SPEED_LIMITED:
- maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
- break;
- case SPEED_VARIABLE:
- maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient));
- break;
- default:
- maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
- break;
+ switch(st_maxSpeed) {
+ case SPEED_LIMITED:
+ maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
+ break;
+ case SPEED_VARIABLE:
+ maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient));
+ break;
+ default:
+ maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
+ break;
}
return;
}
@@ -536,39 +609,40 @@
pc.printf("\r\n Output TROTTLE\r\n");
#endif
switch (st_thro) {
- case UNDER_SPEED:
- if ( (pulseSpeed + p_sectionCourante->us_accel) < MAX_PULSE_WIDTH_FOR_TACHY)
- {
- pulseSpeed += p_sectionCourante->us_accel;
- }
- break;
- case OVER_SPEED:
- if ( (pulseSpeed - p_sectionCourante->us_decel) > INITAL_PULSE_SPEED)
- {
- pulseSpeed -= p_sectionCourante->us_decel;
- }
- break;
- case AT_SPEED:
- break;
- case BRAKING:
+ case UNDER_SPEED:
+ if ( (pulseSpeed_us + p_sectionCourante->coef_p_accel) < MAX_PULSE_WIDTH_FOR_TACHY_US) {
+ pulseSpeed_us += p_sectionCourante->coef_p_accel;
+ }
+ break;
+ case OVER_SPEED:
+ if ( (pulseSpeed_us - p_sectionCourante->coef_p_accel) > INITAL_PULSE_SPEED_US) {
+ pulseSpeed_us -= p_sectionCourante->coef_p_accel;
+ }
+ break;
+ case AT_SPEED:
+ break;
+ case BRAKING:
#if DEBUG > 2
- pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
+ pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
+#endif
+ wait(0.5);
+ pulseSpeed_us = BRAKING_PULSE_US;
+ break;
+ case STOPPED:
+#if DEBUG > 2
+ pc.printf("STOPPED\r\n");
#endif
- pulseSpeed = BRAKING_PWM;
- break;
- default:
- break;
+ pulseSpeed_us = ZERO_PULSE_SPEED_US;
+ break;
+ default:
+ break;
}
- PwmMotor.pulsewidth_us(pulseSpeed);
+ PwmMotor.pulsewidth_us(pulseSpeed_us);
+#ifdef SAMPLING
+ sampleLog();
+#endif
#if DEBUG > 1
- 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
- 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();
+ pc.printf("PWM Thro pulse: %d micros\r\n",pulseSpeed_us);
#endif
return;
}