#define Pin_KNOB1_SIG 5 // Remove
#define Pin_KNOB2_SIG 2 // Remove
// #define Pin_PWM1_Read 5
// #define Pin_PWM2_Read 2
uint8_t Pin_SERVO1_PWM = 0;
uint8_t Pin_SERVO2_PWM = 1;
uint8_t Pin_SERVO3_PWM = 4;
uint8_t Pin_SERVO4_PWM = 3;

int data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int speed[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int tH[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int tL[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int tT[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};

void setup() {
pinMode(Pin_SERVO1_PWM, OUTPUT);
pinMode(Pin_SERVO2_PWM, OUTPUT);
pinMode(Pin_SERVO3_PWM, OUTPUT);
pinMode(Pin_SERVO4_PWM, OUTPUT);
// pinMode(Pin_PWM1_Read, INPUT);
// pinMode(Pin_PWM2_Read, INPUT);
PWM_set();}

void loop() {
DUMMY_write_data(); // Remove
// RC_read_PWM();
SERVO_read_data();}

void PWM_set() {
// TCCR0 -> 5 & 6, TCCR1 -> 9 & 10, TCCR2 -> 11 & 3
// 20 ms (50 Hz) -> ICR1 39999 -> OCR0 156
// 0 deg -> 0.544 ms -> OCR1 1088 -> OCR0 4.243
// 180 deg -> 2.4 ms -> OCR1 4800 -> OCR0 18.72
TCCR0A = _BV(WGM00) | _BV(COM0B1) | _BV(COM0B0);
TCCR0B = _BV(WGM02) | _BV(CS02) | _BV(CS00);
OCR0A = 156;}

void DUMMY_write_data() {
int knob1 = analogRead(Pin_KNOB1_SIG);
int knob2 = analogRead(Pin_KNOB2_SIG);
tH[1] = map(knob1, 0, 1023, 1000, 2000);
tH[2] = map(knob1, 0, 1023, 1000, 2000);}

/*
void RC_read_PWM() {
tH[1] = pulseIn(Pin_PWM1_Read, HIGH);
tH[2] = pulseIn(Pin_PWM2_Read, HIGH);
tL[1] = pulseIn(Pin_PWM1_Read, LOW);
tL[2] = pulseIn(Pin_PWM2_Read, LOW);
for (int i = 1; i < 3; i++) {tT[i] = tH[i] + tL[i];}}
*/

void SERVO_read_data() {
int servo1 = map(tH[1], 1000, 2000, 1088, 4800);
int servo2 = map(tH[1], 1000, 2000, 1088, 4800);
int servo3 = map(tH[2], 1000, 2000, 1088, 4800);
int servo4 = map(tH[2], 1000, 2000, 1088, 4800);
OCR1A = servo1;
OCR1B = servo3;}