Fixes of badly designed current measuring

- removed waiting_for_current_reversation()
- add double check of closing current

SW/Controller.cpp

@@ -29,7 +29,8 @@ static usart_regs uart_regs
 
 Controller::Controller(): servo(&DDRB, PB1, &OCR1A, blue_servo_data),
                           adc(),
-                          uart(uart_regs)
+                          uart(uart_regs),
+                          count(0)
 {
     //Servo ini
     TCCR1A |= (1<<COM1A1) | (1<<COM1B1) | (1<<WGM11);	//Fast PWM non-inverted
@@ -85,9 +86,6 @@ void Controller::gate_handler()
         case WAITING_FOR_CLOSING_CURRENT:       waiting_for_closing_current();
             break;
 
-        case WAITING_FOR_CURRENT_REVERSATION:   waiting_for_current_reversation();
-            break;
-
         case WAITING_FOR_CURRENT_DROP:          waiting_for_current_drop();
             break;
 
@@ -120,16 +118,20 @@ void Controller::waiting_for_closing_current()
 {
     if ((current_negative > ON_TRESHOLD) && (current_negative < 1020))
     {
-        gate_mode = WAITING_FOR_CURRENT_DROP;
+        if (!count)
+        {
+            count++;
+            return;
+        }
+
+        else
+        {
+            count = 0;
+            gate_mode = WAITING_FOR_CURRENT_DROP;
+        }
     }
-}
 
-void Controller::waiting_for_current_reversation()
-{
-    if (current_positive > ON_TRESHOLD)
-    {
-        gate_mode = WAITING_FOR_CURRENT_DROP;
-    }
+    else count = 0;
 }
 
 void Controller::waiting_for_current_drop()
@@ -176,9 +178,6 @@ void Controller::print_debug()
         case WAITING_FOR_CLOSING_CURRENT:       uart.transmit_string("Waiting for closing current");
             break;
 
-        case WAITING_FOR_CURRENT_REVERSATION:   uart.transmit_string("Waiting for current reversation");
-            break;
-
         case WAITING_FOR_CURRENT_DROP:          uart.transmit_string("Waiting for current drop");
             break;
 

SW/Controller.h

@@ -53,6 +53,7 @@ class Controller
     private:
         volatile uint16_t samples[SAMPLES_COUNT];
         volatile uint8_t samples_item;
+        uint8_t count;
 
     public:
         volatile uint16_t current_positive;