/*A robotic gripper is an end effector used in robotic systems to grasp and
manipulate objects. The gripper is typically designed to fit on the end of a 
robot arm and is controlled by servo motors or other actuators. A two-servo 
motor gripper is a type of robotic gripper that uses two servo motors to control
the opening and closing of the gripper and the rotation of the gripper fingers. 
The servo motors are connected to the gripper fingers through linkages, allowing
the gripper to grasp objects of various shapes and sizes.*/

#include <avr/io.h>
#include <avr/interrupt.h>
#define F_CPU 16000000UL
#include <util/delay.h>


volatile uint16_t adc_value0;
volatile uint16_t adc_value1;
volatile int select1 = 0;
volatile int select2 = 0;


ISR(ADC_vect)
{
    if (bit_is_clear(ADCSRA, ADSC)) {
        // conversion complete
        if (ADMUX == 0x40) {
            adc_value1 = ADC;
            // switch to channel 0
            ADMUX = 0x41;
        } else {
            adc_value0 = ADC;
            // switch to channel 1
            ADMUX = 0x40;
        }
        // start next conversion
        ADCSRA |= (1 << ADSC);
    }
}

ISR(INT1_vect) // ISR for Blue Button
{
    if(select1)
    {
    PORTD &= ~(1<<PD6);
    select1 = 0;
    }
    else
    {
    PORTD |= (1<<PD6);
    select1 = 1;
    }
}

ISR(INT0_vect) // ISR for Green button
{
    if(select2)
    {
    PORTD &= ~(1<<PD5);
    select2 = 0;
    }
    else
    {
    PORTD |= (1<<PD5);
    select2 = 1;
    }

}


int main(void)
{
    
    // Pin Setup and Assignments

    //set PD2 (INT0) and PD3 (INT1) as inputs with pull-up resistors
    DDRD &= ~((1 << PD2) & ~(1 << PD3) & ~(1 << PD4) );
    PORTD |= (1 << PD2) | (1 << PD3);

    DDRD |= (1 << PD5) | (1 << PD6) | (1 << PD7);

    // initialize ADC
    ADMUX = 0x40; // Set reference voltage to AVcc with external capacitor at AREF pin    
    ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // enable ADC, enable interrupt, set prescaler to 128
    ADCSRA |= (1 << ADSC); // start first conversion

    
    // enable external interrupts INT0 and INT1
    EIMSK |= (1 << INT0) | (1 << INT1);
    // set interrupt types: Low Level
    EICRA = 0x00;
    

    // Configure timer1 for Fast PWM mode with a frequency of 50Hz
  TCCR1A |= (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11); // Clear OC1A/OC1B on compare match (non-inverting mode)
  TCCR1B |= (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Fast PWM mode, prescaler 8
  ICR1 = 20000; // Set TOP value for 50Hz frequency

  // Set the output pin for the servo motor as a PWM output
  DDRB |= (1 << PB1);
   DDRB |= (1 << PB2);

  uint16_t mapped_ADC1 = 0;
  uint16_t mapped_ADC2 = 0;
    // enable Global interrupts
    sei();

    while (1)
    {
      // Check if the Main Slide switch is ON or OFF
      if(PIND & (1<<PD4)) // if ON run everything
      {
        PORTD |= (1<<PD7); // Turn ON red LED

      if(select1) // for movement and grip
     OCR1A = adc_value1 * 3.94 + 1000;
     if(select2)
     OCR1B = adc_value0 * 3.94 + 1000;
      }
      else // if Main slide switch is OFF
      {
        // Turn OFF everything plus red led
        PORTD &= ~(1<<PD7);
      }
    }
}