#include "servo2.h"

#define FREQ        250   // Sampling frequency

// ----------------------- Variables for servo signal generation -------------
unsigned int  period; // Sampling period
unsigned long loop_timer;
unsigned long motor_timer = 0,servo_timer = 0, motor_diff = 0,servo_diff = 0;  // what is faster?
unsigned long now, difference;
unsigned long add1,add2,add3,add4;
unsigned long esc1 = 1000,
              esc2 = (int)(2500+450)/2,
              esc3 = 1000,
              esc4 = 1000;

bool DEBUG = false; 

tservo servo(12,true),servo2(13,true), servo3(10,true),servo4(11,true);

void setup() {
  // put your setup code here, to run once:
    // Set pins #4 #5 #6 #7 as outputs
    DDRD |= B11110000;

    period = (1000000/FREQ) ; // Sampling period in µs

    // Initialize loop_timer
    loop_timer = micros();
    //Serial.begin(9600);
    add1=add2=add3=add4=10;
    if (DEBUG) Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
  
  esc1 = analogRead(A0);
  esc1 = map(esc1,0,1023,450,2500);
  esc2 = analogRead(A1);
  esc2 = map(esc2,0,1023,450,2500);
  esc3 = analogRead(A2);
  esc3 = map(esc3,0,1023,450,2500);
  esc4 = analogRead(A3);
  esc4 = map(esc4,0,1023,450,2500);


  //Serial.println(esc1);
  motor_timer = micros();
  applyMotorSpeed();
  motor_diff = micros() - motor_timer; 
  /**
  esc1 = esc1 + add1;
  esc2 = esc2 + add2;
  esc3 = esc3 + add3;
  esc4 = esc4 + add4;
  **/
  servo_diff = micros();
  servo = esc1;
  servo2 = esc2;
  servo3 = esc1; 
  servo4 = esc2;  
  servo_diff = micros() - servo_diff;

  if (DEBUG) {
    Serial.print(motor_diff);
    Serial.print(" : ");
    Serial.println(servo_diff);
  }
 // delayMicroseconds(25);

/**
  if(esc1 <= 450 || esc1 >= 2600) {
    add1 = -add1;  
  }
  
  if(esc2 <= 450 || esc2 >= 2600) {
    add2 = -add2;  
  }
  
  if(esc3 <= 450 || esc3 >= 2600) {
    add3 = -add3;  
  }
  
  if(esc4 <= 450 || esc4 >= 2600) {
    add4 = -add4;  
  }
**/  
  
}