TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

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