Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 11:bc24b3ba51a9
- Parent:
- 10:5bf1e6af26c0
- Child:
- 12:51b1b40cc017
--- a/stateMachines.cpp Sat Sep 08 22:24:50 2018 +0000
+++ b/stateMachines.cpp Sun Sep 09 23:14:53 2018 +0000
@@ -1,68 +1,143 @@
#include "states_m.h"
-
-#define NB_ECHANTILLONS_IR 4
-#define NB_ECHANTILLONS_LIDAR 4
-
-static int mode_courant = ; // mettre en define pour la compilation ? ou régler via télécommande ?
-
-uint16_t dist_murGF[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche pour moyenne
-uint16_t dist_murGL[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
-uint16_t dist_murDF[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit pour moyenne
-uint16_t dist_murDL[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
+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
+#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
int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant
int index_fifo_ir = 0;//pour géreer le buffer tournant
int index_fifo_lidar = 0;
+int tmpDist;
+//sections
+s_Section p_section1;
-int tachy_cmps;
//Capteurs
-AnalogIn anaGF(A0);//capteur ir avant gauche 45 deg
-AnalogIn anaGL(A1);//capteur ir coté gauche
-AnalogIn anaDF(A2);//capteur ir avant droit 45deg
-AnalogIn anaDL(A3);//capteur ir coté droit
+AnalogIn anaG90(A0);//capteur ir coté gauche
+AnalogIn anaD90(A1);//capteur ir coté droit
+AnalogIn anaG45(A2);//capteur ir avant gauche 45 deg
+AnalogIn anaD45(A3);//capteur ir avant droit 45deg
+#ifdef DLVV
AnalogIn anaDlvvG(A4);//capteur ir avant droit 10 deg
AnalogIn anaDlvvD(A5);//capteur ir coté droit 10 deg
AnalogIn anaDlvvFront(A6);//capteur ir avant
-Serial device(A4,A5); // tx, rx capteur longue portée
+#endif
Ticker TIMX(TIMX);//compteur de tours
Digital InterruptSW(D0);//interrupt feu DLVV
-int lidarStrength;
-int lidarDist;
+
+//LIDAR
+Serial serialLidar(A4,A5); // tx, rx
+
+int distLidar;// LiDAR actually measured distance value
+int strengthLidar;// LiDAR signal strength
+int check;// check numerical value storage
+int i,j;
+int uart[9];// store data measured by LiDAR
+const int HEADER=0x59;// data package frame header
+
+//SPEED
+int maxSpeed_cmps = 0;
+int tachySpeed_cmps = 0;
+int tachyStepsRegister = 0;
+int tachySectionDist_cm = 0;
+float ms_pwmSpeedPulse = 0.0015;//IDLE
//Etats
-SECTION_ST st_CurrentSection;
-OBSTACLE_ST st_obstacleDet;
MUR_ST st_murs;
+SECTION_ST st_currentSection;
+MAX_SPEED_ST st_maxSpeed;
THROTTLE_ST st_thro;
+#ifdef DLVV
+OBSTACLE_ST st_obstacle;
+#endif
+
+s_Section* p_sectionCourante = NULL;
//time monitoring
-Timer temps;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
+Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
+Timer timerSinceGetTachy;
+
+void getTachySpeed()
+{
+ //tachySteps = VALEUR_DU_PIN;
+ tachySectionDist_cm += tachyStepsRegister * 4;
+ tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us();
+ tachyStepsRegister=0;
+ timerSinceGetTachy.reset();
+ timerSinceGetTachy.start();
+ return;
+}
-
+int getDistMoy(int* tab,int size)
+{
+ int sumMoy = 0;
+ for(int k=0; k<size;k++)
+ {
+ sumMoy+=tab[k];
+ }
+#if (NB_ECHANTILLONS_IR == 4 && NB_ECHANTILLONS_LIDAR == 4)
+ return sumMoy >> 2;
+#else
+ return sumMoy/size;
+#endif
+}
//########## INIT STATES MACHINES ##########
-void murs_init(void)
+void mursInit(void)
{
- st_murs=MUR_GD_OK;
+ st_murs=EQUILIBRAGE_REPULSIF;
return;
}
-void obstacle_init(void)
+#ifdef DLVV
+void obstacleInit(void)
{
- st_obstacleDet=LONG_RANGE_CLEAR;
+ st_speedLimit=FRONT_CLEAR;
return;
}
-void section_init(void)
+#endif
+void sectionInit(void)
{
- temps.reset();
- temps.start();
- st_CurrentSection=ARRET;
+ timeSinceStart.reset();
+ timeSinceStart.start();
+ st_currentSection=ARRET;
+ p_sectionCourante->nextSection=&p_section1;
+ timerSinceGetTachy.start();
+ getTachySpeed();//to reset
+ tachySectionDist_cm = 0;
+ tachyStepsRegister = 0;
+
+ //section de test
+ p_section1.nextSection = NULL;
+ p_section1.targetSpeed_cmps = 50;
+ p_section1.brakingCoefficient =
+ p_section1.ms_accel = 0.00005;
+ p_section1.ms_decel = 0.0001;
+ p_section1.lidarWarningDist_cm = 200;
+ p_section1.slowSpeed_cmps = 15;
+ p_section1.lng_section_cm = 1000;//10m
+ p_section1.coef_p = 1;
+ p_section1.coef_i = 0;
+ p_section1.coef_d = 0;
return;
}
-void throttle_init(void)
+
+void maxSpeedInit(void)
+{
+ st_maxSpeed=SPEED_MAX;
+ maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
+ serialLidar.baud(115200);
+ return;
+}
+
+void throttleInit(void)
{
st_thro = AT_SPEED;
return;
@@ -70,55 +145,258 @@
//########## UPDATE STATES ##########
-void murs_update(void)
+void mursUpdate(void)
{
- MUR_ST st_tmp_murs;
- dist_murGF=anaGF.read_u16();
- dist_murGL=anaGL.read_u16();
- dist_murDF=anaDF.read_u16();
- dist_murDL=anaDL.read_u16();
+ MUR_ST st_tmpMurs;
+ //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();
+ index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
- if(1)
- {
- st_tmp_murs = MUR_GD_OK;
- }else
- {
- st_tmp_murs = MURS_ABSENTS;
+ switch (st_murs) {
+ case EQUILIBRAGE_REPULSIF:
+ tmpDist = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
+ if( tmpDist <= IR_DEADZONE_U16_22cm)
+ {
+ st_tmpMurs = ATTRACTIF_D;
+ }else if(tmpDist <= IR_DEADZONE_U16_22cm)
+ {
+ st_tmpMurs = ATTRACTIF_G;
+ }else
+ {
+ st_tmpMurs = EQUILIBRAGE_REPULSIF;
+ }
+ break;
+ case ATTRACTIF_D:
+ tmpDist= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
+ if( tmpDist >= IR_FAR_U16_105cm)
+ {
+ st_tmpMurs = ATTRACTIF_D;
+ }else
+ {
+ st_tmpMurs = EQUILIBRAGE_REPULSIF;
+ }
+ break;
+ case ATTRACTIF_G:
+ tmpDist= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
+ if(tmpDist >= IR_FAR_U16_105cm)
+ {
+ st_tmpMurs = ATTRACTIF_G;
+ }else
+ {
+ st_tmpMurs = EQUILIBRAGE_REPULSIF;
+ }
+ break;
+ default:
+ return;
+ break;
}
-
- st_murs = st_tmp_murs;
+ st_murs = st_tmpMurs;
return;
}
-void obstacle_update(void)
+#ifdef DLVV
+void obstacleUpdate(void)
{
return;
}
-void section_update(void)
+#endif
+
+void sectionUpdate(void)
{
+ 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;
+ }
+ break;
+ case LOADING_SECTION:
+ if(p_sectionCourante->nextSection != NULL)
+ {
+ 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;
}
-void throttle_update(void)
+void maxSpeedUpdate(void)
{
+ MAX_SPEED_ST st_tmpMaxSpeed;
+ i=0;
+ if (serialLidar.readable()) { //check whether the serial port has data input
+ if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59
+ uart[0]=HEADER;
+ if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59
+ uart[1]=HEADER;
+ for(i=2; i<9; i++) { // store data to array
+ uart[i]=serialLidar.getc();
+ }
+ check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
+ if(uart[8]==(check&0xff)) { // check the received data as per protocols
+ distLidar=uart[2]+uart[3]*256;// calculate distance value
+ strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
+ }
+ }
+ }
+ }
+
+ if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) )
+ {
+ if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm)
+ {
+ st_tmpMaxSpeed = SPEED_VARIABLE;
+ }else
+ {
+ st_tmpMaxSpeed = SPEED_LIMITED;
+ }
+ }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA))
+ {
+ st_tmpMaxSpeed = SPEED_MAX;
+ }
+ else{
+ return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil
+ }
+
+
+ st_maxSpeed = st_tmpMaxSpeed;
+ return;
+}
+
+void throttleUpdate(void)
+{
+ THROTTLE_ST st_tmpThro;
+
+ switch (st_thro) {
+ case UNDER_SPEED:
+ if( tachySpeed_cmps > maxSpeed_cmps )
+ {
+ st_tmpThro = AT_SPEED;
+ }else
+ {
+ return;
+ }
+ break;
+ case OVER_SPEED:
+ if( tachySpeed_cmps < maxSpeed_cmps )
+ {
+ st_tmpThro = AT_SPEED;
+ }else
+ {
+ return;
+ }
+ break;
+ case AT_SPEED:
+ 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;
+ default:
+ break;
+ }
+
+ st_thro = st_tmpThro;
return;
}
//########## OUTPUT STATES ##########
-void murs_output(void)
-{
+//updating output parameters
+void mursOutput(void)
+{
+ switch (st_murs) {
+ case EQUILIBRAGE_REPULSIF:
+ break;
+ case ATTRACTIF_G:
+ break;
+ case ATTRACTIF_D:
+ break;
+ default:
+ break;
+ }
return;
}
-void obstacle_output(void)
+#ifdef DLVV
+void obstacleOutput(void)
{
return;
}
-void section_output(void)
+#endif
+void sectionOutput(void)
{
+
+ switch (st_currentSection) {
+ case RUNNING_SECTION:
+ break;
+ case LOADING_SECTION:
+ p_sectionCourante=p_sectionCourante->nextSection;
+ tachySectionDist_cm = 0;
+ break;
+ case ARRET:
+ wait(10);//on est à l'arret
+ break;
+ default:
+ break;
+ }
return;
}
-void throttle_output(void)
+
+void maxSpeedOutput(void)
{
+ 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;
}
+
+void throttleOutput(void)
+{
+ switch (st_thro) {
+ case UNDER_SPEED:
+ ms_pwmSpeedPulse += p_sectionCourante->ms_accel;
+ break;
+ case OVER_SPEED:
+ ms_pwmSpeedPulse -= p_sectionCourante->ms_decel;
+ break;
+ case AT_SPEED:
+ default:
+ break;
+ }
+ return;
+}