Add lock signal sensing

SW/Controller.cpp

@@ -63,6 +63,13 @@ Controller::Controller(): servo(&DDRB, PB1, &OCR1A, blue_servo_data),
     TCCR0 = (1<<CS02);  //225Hz
     TIMSK |= (1<<TOIE0);
 
+    //Ext IRQ ini
+    MCUCR |= (1<<ISC11);    //Falling edge
+    GICR  |= (1<<INT1);
+
+    //TCC2 ini
+    TIMSK |= (1<<TOIE2);
+
     sei();
 }
 
@@ -72,5 +79,7 @@ void Controller::print_debug()
     uart.transmit_dec32(current_positive);
     uart.transmit_string("\tN: ");
     uart.transmit_dec32(current_negative);
+    uart.transmit_string("\tL: ");
+    uart.transmit_dec32((char)lock_signal);
     uart.transmit_new_line();
 }
\ No newline at end of file

SW/Controller.h

@@ -28,6 +28,9 @@ class Controller
         volatile uint16_t current_negative;
         volatile uint8_t  channel;
 
+        volatile bool lock_signal;
+        volatile uint8_t lock_signal_delay;
+
     //Methods
     public:
         Controller();
@@ -37,6 +40,34 @@ class Controller
             adc.start_conversion();
         }
 
+        void lock_signal_delay_handler()
+        {
+            lock_signal_delay++;
+
+            if (lock_signal_delay > 224)    //cca 4s
+            {
+                if (PIND & (1<<PD3))
+                {
+                    lock_signal_delay = 0;
+                    TCCR2 = 0;
+                    TCNT2 = 0;
+                    lock_signal = false;
+                }
+
+                else lock_signal_delay = 0;
+
+            }
+        }
+
+        void lock_signal_handler()
+        {
+            if (!lock_signal)
+            {
+                lock_signal = true;
+                TCCR2 = (1<<CS22) | (1<<CS21) | (1<<CS20);
+            }
+        }
+
         void adc_handler()
         {
             switch (channel)

SW/irq.cpp

@@ -10,6 +10,16 @@ ISR(TIMER0_OVF_vect)
     controller.timer0_handler();
 }
 
+ISR(TIMER2_OVF_vect)
+{
+    controller.lock_signal_delay_handler();
+}
+
+ISR(INT1_vect)
+{
+    controller.lock_signal_handler();
+}
+
 ISR(ADC_vect)
 {
     controller.adc_handler();

SW/irq.h

@@ -3,5 +3,7 @@
 #include <avr/interrupt.h>
 
 ISR(TIMER0_OVF_vect);
+ISR(TIMER2_OVF_vect);
+ISR(INT1_vect);
 ISR(ADC_vect);
 ISR(BADISR_vect);
\ No newline at end of file