Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 22:26fc6e6f7a55
- Parent:
- 21:de7a0a47f8a3
- Child:
- 23:04d393220daa
--- a/stateMachines.cpp Sat Sep 15 12:58:47 2018 +0000
+++ b/stateMachines.cpp Sat Sep 15 15:56:01 2018 +0000
@@ -4,6 +4,7 @@
#endif
#if DEBUG >= -1
InterruptIn my_button(USER_BUTTON);
+Timer timerLog;
#endif
uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
@@ -38,14 +39,16 @@
int pulseSpeed_us = INITAL_PULSE_SPEED_US;
//Capteurs direction
-AnalogIn anaG90(PC_1);//capteur ir coté gauche
-AnalogIn anaD90(PA_1);//capteur ir coté droit
-AnalogIn anaG45(PC_0);//capteur ir avant gauche 45 deg
-AnalogIn anaD45(PC_2);//capteur ir avant droit 45deg
+
+
+AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche
+AnalogIn anaD90(CAPT_90_DROITE);//capteur ir coté droit
+AnalogIn anaG45(CAPT_45_GAUCHE);//capteur ir avant gauche 45 deg
+AnalogIn anaD45(CAPT_45_DROITE);//capteur ir avant droit 45deg
#ifdef DLVV
-AnalogIn anaDlvvG(PB_0);//capteur ir avant droit 10 deg
-AnalogIn anaDlvvD(PC_3);//capteur ir coté droit 10 deg
-AnalogIn anaDlvvFront(PA_4);//capteur ir avant
+AnalogIn anaDlvvG(CAPT_10_GAUCHE);//capteur ir avant droit 10 deg
+AnalogIn anaDlvvD(CAPT_10_DROITE);//capteur ir coté droit 10 deg
+AnalogIn anaDlvvFront(CAPT_DEVANT);//capteur ir avant
#endif
int differenceGD45,differenceGD90,prevDiffGD90,prevDiffGD45,derive45,derive90;
@@ -72,7 +75,6 @@
int tachySpeed_cmps = 0;
int tachyStepsRegister = 0;
int tachySectionDist_cm = 0;
-int currentSpeedFromZero_us = INITAL_PULSE_SPEED_US;
//Etats
MUR_ST st_murs;
@@ -111,9 +113,6 @@
history[m].diffgd90 = 0;
history[m].pwm_thro_us = 0;
history[m].pwm_dir_us = 0;
- history[m].lidarDist = 0;
- history[m].lidarStr = 0;
- history[m].speed = 0;
}
#if DEBUG > 0
pc.printf("[INIT SAMPLE DONE]\r\n");
@@ -122,52 +121,50 @@
}
void sampleLog(void)
{
- if(indexSample < TAILLE_SAMPLES) {
+ if(timerLog.read_us() > 1000) {
+ timerLog.reset();
+ timerLog.start();
+ if(indexSample < TAILLE_SAMPLES) {
#ifdef DLVV
- history[indexSample].states.murs_dlvv = (char)st_obstacle;
+ history[indexSample].states.murs_dlvv = (char)st_obstacle;
#else
- history[indexSample].states.murs_dlvv = (char)st_murs;
+ 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].diffgd45 = differenceGD45;
- history[indexSample].diffgd90 = differenceGD90;
- history[indexSample].pwm_thro_us = pulseSpeed_us;
- history[indexSample].pwm_dir_us = pulseDirection_us;
- history[indexSample].lidarDist = distLidar;
- history[indexSample].lidarStr = strengthLidar;
- history[indexSample].speed = tachySpeed_cmps;
- history[indexSample].dist = tachySectionDist_cm,
- indexSample++;
+ 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].diffgd45 = differenceGD45;
+ history[indexSample].diffgd90 = differenceGD90;
+ history[indexSample].pwm_thro_us = pulseSpeed_us;
+ history[indexSample].pwm_dir_us = pulseDirection_us;
+ 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();
+ 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;
}
- return;
}
void transmitData(void)
{
#if DEBUG > 0
pc.printf("[START TO TRANSMIT]\r\n");
#endif
- serialLidar.printf("time,diffgd 45,diffgd 90,pwm_thro_us,pwm_dir_us,dist,lidar dist,strength,speed,murs/dlvv,section,maxSpeed,throttle\r\n");
+ serialLidar.printf("time,diffgd 45,diffgd 90,pwm_thro_us,pwm_dir_us,dist,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,%d\r\n",
+ serialLidar.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
history[p].time,
history[p].diffgd45,
history[p].diffgd90,
history[p].pwm_thro_us,
history[p].pwm_dir_us,
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,
@@ -221,7 +218,7 @@
void it4cm()
{
- tachyStepsRegister+=4;
+ tachyStepsRegister+=TACHY_CM;
#if DEBUG > 0
pc.printf("IT tachy\r\n");
#endif
@@ -290,12 +287,12 @@
//section de test
p_section1.nextSection = NULL;
- p_section1.targetSpeed_cmps = 10;
- p_section1.slowSpeed_cmps = 5;
- p_section1.brakingCoefficient = 30; // application de la formule
+ p_section1.targetSpeed_cmps = 328;
+ p_section1.slowSpeed_cmps = 328;
+ p_section1.brakingCoefficient = 0; // application de la formule
p_section1.coef_p_speed = 1;
- p_section1.lidarWarningDist_cm = 60;
- p_section1.lng_section_cm = 160;//1600cm
+ 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;
@@ -334,6 +331,10 @@
timeSinceStart.reset();
timeSinceStart.start();
#endif
+#if DEBUG >= -1
+ timerLog.start();
+#endif
+
return;
}
@@ -362,9 +363,9 @@
case EQUILIBRAGE_REPULSIF:
distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
- if( distMurG90Moy <= IR_DEADZONE_U16_22cm) {
+ if( distMurG90Moy > IR_DEADZONE_U16_22cm) {
st_tmpMurs = ATTRACTIF_D;
- } else if(distMurD90Moy <= IR_DEADZONE_U16_22cm) {
+ } else if(distMurD90Moy > IR_DEADZONE_U16_22cm) {
st_tmpMurs = ATTRACTIF_G;
} else {
st_tmpMurs = EQUILIBRAGE_REPULSIF;
@@ -464,17 +465,16 @@
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 ) { //|| 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) {
+ } 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)) {
+ 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;
@@ -487,14 +487,13 @@
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;
case STOPPED:
if(st_currentSection == RUNNING_SECTION) {
- st_tmpThro = REGULATION_SPEED;
+ st_tmpThro = REGULATION_SPEED;
} else {
st_tmpThro = STOPPED;
}
@@ -523,32 +522,32 @@
case ATTRACTIF_G:
differenceGD90 = IR_DEADZONE_U16_22cm - distMurG90Moy;
differenceGD45 = IR_DEADZONE_U16_22cm - distMurG45Moy;
- break;
+ break;
case ATTRACTIF_D:
differenceGD90 = distMurD90Moy -IR_DEADZONE_U16_22cm;
differenceGD45 = distMurD45Moy - IR_DEADZONE_U16_22cm;
- break;
+ break;
default:
break;
}
-
+
//deriv correction
- derive45 = differenceGD45 - prevDiffGD45;
- derive90 = differenceGD90 - prevDiffGD90;
+ derive45 = differenceGD45 - prevDiffGD45;
+ derive90 = differenceGD90 - prevDiffGD90;
//integral correction
- lastDifferences90[lastDifferenceIndex] = differenceGD90;
- integralSum=0;
- for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
- integralSum+=lastDifferences90[f];
- }
- lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
-
+ lastDifferences90[lastDifferenceIndex] = differenceGD90;
+ integralSum=0;
+ for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
+ integralSum+=lastDifferences90[f];
+ }
+ lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
+
//application des coefficients
pulseDirection_us = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p)
+ ( ((derive90*2+derive90*4)/8) / p_sectionCourante->coef_d)
+ (integralSum / p_sectionCourante->coef_i)
- + 1500;
-
+ + DIRECTION_PULSE_MIDDLE;
+
//gestioon du dépassement
if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
#if DEBUG > 1
@@ -622,8 +621,7 @@
#endif
switch (st_thro) {
case REGULATION_SPEED:
- currentSpeedFromZero_us = tachySpeed_cmps * COEF_CMPS_TO_PULSE / p_sectionCourante->coef_p_speed;
- pulseSpeed_us = currentSpeedFromZero_us + INITAL_PULSE_SPEED_US;
+ pulseSpeed_us = maxSpeed_cmps * 279 / 2048 + 1558 ;
break;
case AT_SPEED:
break;
@@ -643,7 +641,7 @@
default:
break;
}
-
+
PwmMotor.pulsewidth_us(pulseSpeed_us);
#ifdef SAMPLING
sampleLog();