#include <Arduino.h>
#include "PinChangeInt.h"
#include "Pins.h"
#include "BalanceCar.h"
#include "RF24Reciever.h"

int cnt = 0;

void setup()
{
  carInitialize();
  RF24setup();
  timer();
}

void loop(){

joyStickDriving();
}

void timer(){
  // Example of timer from https://www.instructables.com/Arduino-Timer-Interrupts/
  
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  OCR1A = 1249;// = (16*10^6) / (1*1024) - 1 
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS01) | (1 << CS00);  
  TIMSK1 |= (1 << OCIE1A);

  TCCR2A = 0;
  TCCR2B = 0;
  TCNT2  = 0;
  OCR2A = 250;// = (16*10^6) / (1000*64) - 1
  TCCR2A |= (1 << WGM21);
  TCCR2B |= (1 << CS22);   
  TIMSK2 |= (1 << OCIE2A);

}

ISR(TIMER1_COMPA_vect){
  balanceCar();
}

ISR(TIMER2_COMPA_vect){
  cnt++;
  if (cnt >= 100){
    cnt = 0;
    RF24loop();
  }
}