/*
 * Delay_by_trigger_out.c
 *
 * Created: 2016-12-21 18:02:18
 *  Author: Almighty
 */ 

#define FOSC 8000000 // Clock Speed
#define BAUD 9600
#define MYUBRR FOSC/16/BAUD-1

#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#include <math.h>
//---
#ifndef F_CPU
#define F_CPU 8000000UL // or whatever may be your frequency
#endif
// remember to put it before delay.h
//---
#include <util/delay.h>
//#include iomx8.h"
/*****************************************/
//Macro för bit operationer
#define BIT(x) (1 << (x))
#define SETBITS(x,y) ((x) |= (y))
#define CLEARBITS(x,y) ((x) &= (~(y)))
#define SETBIT(x,y) SETBITS((x), (BIT((y))))
#define CLEARBIT(x,y) CLEARBITS((x), (BIT((y))))
#define BITSET(x,y) ((x) & (BIT(y)))
#define BITCLEAR(x,y) !BITSET((x), (y))
#define BITSSET(x,y) (((x) & (y)) == (y))
#define BITSCLEAR(x,y) (((x) & (y)) == 0)
#define BITVAL(x,y) (((x)>>(y)) & 1)
/*****************************************/
void port_init(void)
{
	
	DDRB |= (0<<PB3) |(1<<PB4);		//PB3 input trigger PB4 servo PPM switch pusher
	
	PORTB |= (0<<PB3);					//pulldown	PB3 rigger in
	//PB3 input

}
void push_and_release()
{
	for (int i=0;i < 10;i++) //unlock lid
	{
		SETBIT(PORTB, 4);
		_delay_us(1700);
		CLEARBIT(PORTB, 4);
		_delay_us(18300);
	}
	_delay_ms(500);
	for (int i=0;i < 20;i++) //lock lid
	{
		SETBIT(PORTB, 4);
		_delay_us(700);
		CLEARBIT(PORTB, 4);
		_delay_us(19300);
	}
}

int trigger_active_or_not()
{
	int yes_or_no;
	if (PINB & (1 << PINB3))
	{
		yes_or_no = 1;
	}
	else yes_or_no = 0;
	
	return yes_or_no;
}

int main(void)
{
	port_init();
	
	int time_to_start;		// 1 = yes
	int last_thing_todo = 0; 
   
    while(1)
    {
       time_to_start = trigger_active_or_not();
	   if(time_to_start == 1)
	   {
			_delay_ms(10000); // this is the delay between servos  first amplifier 7000ms second amplifier 10000ms
		    push_and_release();
		   
		   last_thing_todo = 1;
		   while(time_to_start == 1)
		   {
				time_to_start = trigger_active_or_not();
				last_thing_todo = 1;
		   }
	   
				if(last_thing_todo == 1) 
				{
					last_thing_todo = 0;
					_delay_ms(10000);
					push_and_release();
				}
	   }
	   
    }
	
	
}