added arduino code - not tested in real life

arduino_code/adapter_motor/adapter_motor.ino

+//timer setup for timer4
+//For arduino Mega
+
+//timer4 will interrupt at 20Hz
+
+//storage variable
+const byte INTERRUPT_NOISE_THRESHOLD = 5 ;
+
+const word MOTOR_CENTER = 1000 ;
+const word MOTOR_RIGHT = 1300 ;
+const word MOTOR_LEFT = 700 ;
+const byte MOTOR_THRESHOLD = 10 ;
+const word MOTOR_RANGE = MOTOR_RIGHT - MOTOR_LEFT ;
+
+const word RADIO_CENTER = 1500 ;
+const word RADIO_RIGHT = 1900 ;
+const word RADIO_LEFT = 1100 ;
+const byte RADIO_THRESHOLD = 10 ;
+const word RADIO_RANGE = RADIO_RIGHT - RADIO_LEFT ;
+
+const int _kp = 1;
+
+const byte interrupt_pin = 2 ;
+static volatile uint32_t delta_us ;
+static volatile uint32_t prev_change_date_us = 0 ;
+static volatile uint32_t pos = 0;
+static volatile byte count = 0;
+static volatile uint32_t recv = 0;
+
+bool first = true;
+bool started = false;
+static volatile int scale_pos = 50;
+static volatile int scale_radio = 50;
+byte speed_command = 0;
+//***********************************************************************//
+void setup(){
+// put your setup code here, to run once:
+  Serial.begin(9600); //usb pc
+  Serial1.begin(9600); //sabertooth driver
+  Serial2.begin(115200); //multiplexer
+  Serial1.write(speed_command);
+  while(!Serial){
+    
+  }
+  cli(); //stop interrupts
+  
+  //set timer4 interrupt at 20Hz
+  TCCR4A = 0;// set entire TCCR4A register to 0
+  TCCR4B = 0;// same for TCCR4B
+  TCNT4  = 0;//initialize counter value to 0
+  // set compare match register for 20hz increments
+  OCR4A = 311/1;// = (16*10^6) / (20*1024) - 1 (must be <65536) // 780 = 200hz / // turn on CTC mode
+  TCCR4B |= (1 << WGM12);
+  // Set CS12 and CS10 bits for 1024 prescaler
+  TCCR4B |= (1 << CS12) | (1 << CS10);  
+  // enable timer compare interrupt
+  TIMSK4 |= (1 << OCIE4A);
+  
+  sei();//allow interrupts
+
+  attachInterrupt(digitalPinToInterrupt(interrupt_pin),pwm,CHANGE);
+  //delay(5);
+}//end setup
+
+ISR(TIMER4_COMPA_vect){
+  //Serial.print("radio: ");
+  //Serial.println(scale_radio);
+  Serial2.write(0x43);
+  count = 0;
+  first = true;
+}
+//***********************************************************************//
+void loop(){
+ //do other things here
+}
+//***********************************************************************//
+void pwm() {
+  //Serial.print("change");
+  uint32_t curr_date = micros () ;
+  bool state_us ;
+
+  delta_us = curr_date - prev_change_date_us ;
+  
+  if (delta_us < INTERRUPT_NOISE_THRESHOLD) return ;
+  /* Record the previous pin state since it has just changed. */
+  state_us = ! digitalRead (interrupt_pin) ;
+  /* Ensure that we have detected a falling edge, i.e. the previous level
+     of the pin was HIGH. */
+  if (state_us) {
+    //Serial.print("radio: ");
+    //Serial.println(delta_us);
+    if (delta_us <= RADIO_CENTER + RADIO_THRESHOLD && delta_us >= RADIO_CENTER - RADIO_THRESHOLD){
+      scale_radio = 50;
+    }
+    else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD){
+      scale_radio = 0;
+    }
+    else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD){
+      scale_radio = 100;
+    } else {
+      scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE);
+    }
+    //Serial.print("scale_radio: ");
+    //Serial.println(scale_radio);
+  }
+  /* Change the date anyway. */
+  prev_change_date_us = curr_date ;
+}
+//***********************************************************************//
+void serialEvent2() {
+  while(Serial2.available()){
+    if (first) {
+      recv = Serial2.read();
+      first = false;
+    }
+    else {
+      recv = (recv << 8) + Serial2.read();
+    }
+    count++;
+  }
+  if (count == 4) {
+    pos = recv;
+    //Serial.print("scale_pos: ");
+    //Serial.println(pos);
+    count = 0;
+    speed_command = 64;
+    first = true;
+    if (!started) {
+      if (pos > MOTOR_CENTER + MOTOR_THRESHOLD || pos < MOTOR_CENTER - MOTOR_THRESHOLD){
+        if(pos < MOTOR_LEFT) speed_command = 127;
+        else if(pos > MOTOR_RIGHT) speed_command = 1;
+        else if(pos < MOTOR_CENTER - MOTOR_THRESHOLD) speed_command = 100;
+        else if(pos > MOTOR_CENTER + MOTOR_THRESHOLD) speed_command = 28;
+      } else {
+        //Serial.print("centered");
+        speed_command = 64;
+        started = true;
+      }
+      Serial1.write(speed_command);
+    }
+    else {
+      scale_pos = (pos - MOTOR_LEFT)* 100/(MOTOR_RANGE);
+      
+      //Serial.print("scale_pos: ");
+      //Serial.println(scale_pos);
+      
+      int error_pos = scale_radio - scale_pos;
+      int v = _kp * error_pos;
+      if (v >= -1 && v <= 1){
+        speed_command = 64;
+      }
+      else if (v >= 30) {
+        speed_command = 127;
+      }
+      else if (v <= -30) {
+        speed_command = 1;
+      }
+      else {
+        speed_command = (1.26) * (v - 50) + 127;
+      }
+      //Serial.print("v: ");
+      //Serial.println(v);
+      //Serial.print("speed_command: ");
+      //Serial.println(speed_command);
+    }
+    Serial1.write(speed_command);
+  }
+}
+//***********************************************************************//
+void serialEvent(){
+  started = true;
+  Serial1.write(0);
+}