Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 23:04d393220daa
- Parent:
- 22:26fc6e6f7a55
- Child:
- 24:698fefbbee00
--- a/stateMachines.cpp Sat Sep 15 15:56:01 2018 +0000
+++ b/stateMachines.cpp Sun Sep 16 15:04:41 2018 +0000
@@ -4,8 +4,10 @@
#endif
#if DEBUG >= -1
InterruptIn my_button(USER_BUTTON);
+#ifdef DUMP_SAMPLIG_PERIOD
Timer timerLog;
#endif
+#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
@@ -20,16 +22,17 @@
uint16_t distMurD90Moy;
uint16_t distMurG45Moy;
uint16_t distMurD45Moy;
+
#ifdef DLVV
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
int index_fifo_lidar = 0;
//sections
s_Section p_section1;
+s_Section p_section2;
//PWM Controls
PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4
@@ -64,7 +67,9 @@
Serial serialLidar(PC_10,PC_11); // tx, rx
int distLidar;// LiDAR actually measured distance value
+int distLidarPrev;
int strengthLidar;// LiDAR signal strength
+int strengthLidarPrev;
int check;// check numerical value storage
int i;
int uart[9];// store data measured by LiDAR
@@ -113,6 +118,8 @@
history[m].diffgd90 = 0;
history[m].pwm_thro_us = 0;
history[m].pwm_dir_us = 0;
+ history[m].distLidar = 0;
+ history[m].strLidar = 0;
}
#if DEBUG > 0
pc.printf("[INIT SAMPLE DONE]\r\n");
@@ -121,9 +128,11 @@
}
void sampleLog(void)
{
- if(timerLog.read_us() > 1000) {
+#ifdef DUMP_SAMPLIG_PERIOD
+ if(timerLog.read_us() > DUMP_SAMPLIG_PERIOD) {
timerLog.reset();
timerLog.start();
+#endif
if(indexSample < TAILLE_SAMPLES) {
#ifdef DLVV
history[indexSample].states.murs_dlvv = (char)st_obstacle;
@@ -139,7 +148,9 @@
history[indexSample].pwm_thro_us = pulseSpeed_us;
history[indexSample].pwm_dir_us = pulseDirection_us;
history[indexSample].dist = tachySectionDist_cm,
- indexSample++;
+ history[indexSample].distLidar = distLidar;
+ history[indexSample].strLidar = strengthLidar;
+ 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);
@@ -149,7 +160,9 @@
#endif
}
return;
+#ifdef DUMP_SAMPLIG_PERIOD
}
+#endif
}
void transmitData(void)
{
@@ -235,7 +248,9 @@
}
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
+ distLidarPrev = distLidar;
distLidar=uart[2]+uart[3]*256;// calculate distance value
+ strengthLidarPrev = strengthLidar;
strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
}
}
@@ -285,17 +300,30 @@
tachySectionDist_cm = 0;
tachyStepsRegister = 0;
- //section de test
- p_section1.nextSection = NULL;
- p_section1.targetSpeed_cmps = 328;
+ //section de test 1
+ p_section1.nextSection =NULL;// &p_section2;
+ p_section1.targetSpeed_cmps = 400;
p_section1.slowSpeed_cmps = 328;
- p_section1.brakingCoefficient = 0; // application de la formule
+ p_section1.brakingCoefficient = 61; // application de la formule
p_section1.coef_p_speed = 1;
- p_section1.lidarWarningDist_cm = 100;
- p_section1.lng_section_cm = 500;//500cm
- p_section1.coef_p = 35;
- p_section1.coef_i = 35*NB_INTEGRAL_SAMPLES;
- p_section1.coef_d = 35;
+ p_section1.lidarWarningDist_cm = 120;
+ p_section1.lng_section_cm = 30000;//30m
+ p_section1.coef_p = 50;//35;
+ p_section1.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES;
+ p_section1.coef_d = 10000;//35;
+
+ //section de test
+ p_section2.nextSection = NULL;
+ p_section2.targetSpeed_cmps = 328;
+ p_section2.slowSpeed_cmps = 328;
+ p_section2.brakingCoefficient = 0; // application de la formule
+ p_section2.coef_p_speed = 1;
+ p_section2.lidarWarningDist_cm = 300;
+ p_section2.lng_section_cm = 200;//2m
+ p_section2.coef_p = 50;
+ p_section2.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES;
+ p_section2.coef_d = 10000;//35;
+
return;
}
@@ -331,7 +359,7 @@
timeSinceStart.reset();
timeSinceStart.start();
#endif
-#if DEBUG >= -1
+#ifdef DUMP_SAMPLIG_PERIOD
timerLog.start();
#endif
@@ -351,46 +379,12 @@
distMurG45[index_fifo_ir]=anaG45.read_u16();
distMurD45[index_fifo_ir]=anaD45.read_u16();
index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
-
+
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
+ distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
+ distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
- 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;
- }
- st_murs = st_tmpMurs;
return;
}
#ifdef DLVV
@@ -441,10 +435,8 @@
#endif
i=0;
- if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) {
- if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm) {
- st_tmpMaxSpeed = SPEED_WARNING;
- } else if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
+ if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) {
+ if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
st_tmpMaxSpeed = SPEED_MAX;
} else {
st_tmpMaxSpeed = BLOCKED;
@@ -465,29 +457,20 @@
getTachySpeed();
switch (st_thro) {
case REGULATION_SPEED:
- if(st_currentSection == ARRET || st_maxSpeed == BLOCKED ) { //|| tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS))
+ if(st_currentSection == ARRET || st_maxSpeed == BLOCKED ) {
st_tmpThro = BRAKING;
- } else if( tachySpeed_cmps <= maxSpeed_cmps + SPEED_DELTA_CMPS && tachySpeed_cmps >= maxSpeed_cmps - SPEED_DELTA_CMPS) {
- st_tmpThro = AT_SPEED;
- } else {
- return;
- }
- break;
- case AT_SPEED:
- if(st_currentSection == ARRET || st_maxSpeed == BLOCKED ) { //|| tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS))
- st_tmpThro = BRAKING;
- } else if(!(tachySpeed_cmps <= maxSpeed_cmps + SPEED_DELTA_CMPS && tachySpeed_cmps >= maxSpeed_cmps - SPEED_DELTA_CMPS)) {
- st_tmpThro = REGULATION_SPEED;
} else {
return;
}
break;
case BRAKING:
+ if( strengthLidar > LIDAR_STRENGTH_THRESOLD && strengthLidarPrev > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm && distLidarPrev-distLidar > 0)
+ {
+ st_tmpThro = BRAKING;
+ }else
if(st_currentSection == ARRET) {
st_tmpThro = STOPPED;
- } else if(tachySpeed_cmps <= maxSpeed_cmps + SPEED_DELTA_CMPS && tachySpeed_cmps >= maxSpeed_cmps - SPEED_DELTA_CMPS) {
- st_tmpThro = AT_SPEED;
- } else {
+ } else{
st_tmpThro = REGULATION_SPEED;
}
break;
@@ -514,22 +497,10 @@
#if (DEBUG > 3)
pc.printf("\r\n Output MURS\r\n");
#endif
- switch (st_murs) {
- case EQUILIBRAGE_REPULSIF:
- differenceGD90 = distMurD90Moy - distMurG90Moy;
- differenceGD45 = distMurD45Moy - distMurG45Moy;
- break;
- case ATTRACTIF_G:
- differenceGD90 = IR_DEADZONE_U16_22cm - distMurG90Moy;
- differenceGD45 = IR_DEADZONE_U16_22cm - distMurG45Moy;
- break;
- case ATTRACTIF_D:
- differenceGD90 = distMurD90Moy -IR_DEADZONE_U16_22cm;
- differenceGD45 = distMurD45Moy - IR_DEADZONE_U16_22cm;
- break;
- default:
- break;
- }
+
+ differenceGD90 = distMurD90Moy - distMurG90Moy;
+ differenceGD45 = distMurD45Moy - distMurG45Moy;
+
//deriv correction
derive45 = differenceGD45 - prevDiffGD45;
@@ -544,7 +515,7 @@
//application des coefficients
pulseDirection_us = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p)
- + ( ((derive90*2+derive90*4)/8) / p_sectionCourante->coef_d)
+ + ( derive45 / p_sectionCourante->coef_d)
+ (integralSum / p_sectionCourante->coef_i)
+ DIRECTION_PULSE_MIDDLE;
@@ -623,8 +594,6 @@
case REGULATION_SPEED:
pulseSpeed_us = maxSpeed_cmps * 279 / 2048 + 1558 ;
break;
- case AT_SPEED:
- break;
case BRAKING:
#if DEBUG > 2
pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
@@ -635,7 +604,7 @@
#if DEBUG > 2
pc.printf("STOPPED\r\n");
#endif
- wait(0.2);
+ wait(0.7);//duree freinage
pulseSpeed_us = ZERO_PULSE_SPEED_US;
break;
default:
@@ -643,9 +612,6 @@
}
PwmMotor.pulsewidth_us(pulseSpeed_us);
-#ifdef SAMPLING
- sampleLog();
-#endif
#if DEBUG > 1
pc.printf("PWM Thro pulse: %d micros\r\n",pulseSpeed_us);
#endif