// Receiver pulse input by means of ISR on hardware interrupt pins 2 and 3 for Uno/Nano
// Timer is set to run at double speed, all pulse counts are two times normal
// Servo outputs use delayMicroseconds(), which also runs at double speed, hence outputs are normal again.
// The loop synchronizes on completion of the ch1 pulse count, hence ch1 must be connected to a valid Rx channel.
// By Max Zuijdendorp, July 2022. Based on various sketches by Phil G.
volatile unsigned long timer_ch1, timer_ch2; // all timer variables are unsigned long
volatile int pulse_time1 = 3000, pulse_time2 = 3000, outpulse1 = 3000, outpulse2 = 3000;
volatile byte ch1Was = 0, ch2Was = 0, sync = 0;
int prevpulse1, prevpulse2, slowstep = 50;
void setup()
{
// set up pin 2 and pin 3 as inputs from Rx (interrupt pins)
pinMode(2, INPUT); // channel 1 from Rx
pinMode(3, INPUT); // channel 2 from Rx
// set up A1 and A2 as servo outputs
pinMode(A1, OUTPUT);
pinMode(A2, OUTPUT);
timer_ch1 = 0; timer_ch2 = 0; // ?
TCCR1A = 0; //reset timer1
TCCR1B = 0;
TCCR1B |= (1 << CS11); //set timer1 to increment every 0,5 us (clkI/O/8 from prescaler)
attachInterrupt(0, read_pwm, CHANGE); // Pin 2 = interrupt 0
attachInterrupt(1, read_pwm, CHANGE); // Pin 3 = interrupt 1
}
void loop()
{
while (sync == 0); // ISR sets sync to true when reading ch1 pulse is complete, loop starts.
sync = 0; // stops loop after one run.
delay(6); // do the o/p pulses mid-frame, reduces jitter...
cli();
outpulse1 = pulse_time1; // do an atomic copy in the quickest way
outpulse2 = pulse_time2;
sei();
map(outpulse1, 2000, 4000, 1040, 4700); // pulse enlarged to 520 - 2350 microseconds x 2
map(outpulse2, 2000, 4000, 1040, 4700); // pulse enlarged to 520 - 2350 microseconds x 2
if (outpulse1-prevpulse1 >= slowstep) outpulse1 = prevpulse1 + slowstep; // restrict count rise to slowstep value
else if(outpulse1-prevpulse1 <= -slowstep) outpulse1 = prevpulse1 - slowstep;// restrict count fall to slowstep value
if (outpulse1-prevpulse2 >= slowstep) outpulse2 = prevpulse2 + slowstep; // restrict count rise to slowstep value
else if(outpulse2-prevpulse2 <= -slowstep) outpulse2 = prevpulse2 - slowstep;// restrict count fall to slowstep value
// if the last input differs less than the slowstep value from the last step up or down, it remains as is.
prevpulse1 = outpulse1; // keep pulse value for next comparison
prevpulse2 = outpulse2; // keep pulse value for next comparison
constrain(outpulse1, 1040, 4700); // pulse counts are twice normal
constrain(outpulse2, 1040, 4700);
digitalWrite(A1, 1); delayMicroseconds(outpulse1); digitalWrite(A1, 0); // servo ch1 out on pin A1
digitalWrite(A2, 1); delayMicroseconds(outpulse2); digitalWrite(A2, 0); // servo ch2 out on pin A2
}
// ISR, same for both interrupts. Only one sync on completion pulse for ch1
void read_pwm(){
if (PIND & 0b00000100) { // if pin 2 or 3 is interrupting and pin 2 = true
if (ch1Was == 0) { // and if pin 2 was false previously
timer_ch1 = micros(); // start timer_ch1
ch1Was = 1;
}
} else {
if (ch1Was == 1) { // if pin 2 was already true, read timer_ch1
pulse_time1 = ((volatile int)micros() - timer_ch1);
ch1Was = 0;
sync = 1; // start loop
}
}
if (PIND & 0b00001000) { // if pin 2 or 3 is interrupting and pin 3 = true
if (ch2Was == 0) {
timer_ch2 = micros();
ch2Was = 1;
}
} else {
if (ch2Was == 1) {
pulse_time2 = ((volatile int)micros() - timer_ch2);
ch2Was = 0;
}
}
}
