........... Motor Control Source Code ............

========== File-Name: 22_Servo_Control.ino ===========

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++ Begin Servo Control Function +++++++++++++++++++
//               Last Update: 11-09-2022  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void Servo_Control()
  {
//   Call Emergency_Motor_Control function to Rev-Up and Rev-Down motor  
     Emergency_Motor_Control(); 

//..............................................................
//   int Servo_MAX_IN = 2400; //1600 + 800
//   int Servo_MIN_IN =  800; //1600 - 800  

//  Calculate the PWM-pulse for Servo_0 => Ailerons
    Servo_0_In = pid_roll_output; 
//    if (Servo_0_In > Servo_MAX_IN) Servo_0_In = Servo_MAX_IN;   
//    if (Servo_0_In < Servo_MIN_IN) Servo_0_In = Servo_MIN_IN;   

//  Calculate the PWM-pulse for Servo_1 => Elevator
      Servo_1_In = pid_pitch_output; 
//    if (Servo_1_In > Servo_MAX_IN) Servo_1_In = Servo_MAX_IN;   
//    if (Servo_1_In < Servo_MIN_IN) Servo_1_In = Servo_MIN_IN;   

//  Calculate the PWM-pulse for Servo_2 => Rudder
    Servo_2_In = pid_yaw_output;
//  if (Servo_2_In > Servo_MAX_IN) Servo_2_In = Servo_MAX_IN;   
//  if (Servo_2_In < Servo_MIN_IN) Servo_2_In = Servo_MIN_IN;   

//  Calculate the PWM-pulse for Servo_3 => Motor Control
    Servo_3_In = Emrev_up_Motor;
//    if (Servo_3_In > Servo_MAX_IN) Servo_3_In = Servo_MAX_IN;   
//    if (Servo_3_In < Servo_MIN_IN) Servo_3_In = Servo_MIN_IN;

 if (Flag==0)
    {
     Servo_0_In=1600;  //roll
     Servo_1_In=1600;  //pitch
     Servo_2_In=1600;  //yaw
     Servo_3_In=1600;  //motor 
    }

   int  servo_run=1;
//      set to 0 servo's are off, 1 servo's are on
   if (servo_run==1)
    {
     Servo_0.writeMicroseconds(Servo_0_In);  //roll
     Servo_1.writeMicroseconds(Servo_1_In);  //pitch
     Servo_2.writeMicroseconds(Servo_2_In);  //yaw
     Servo_3.writeMicroseconds(Servo_3_In);  //motor 
    }

// PWM Teensy-Pin 2 controls Servo_0 => Ailerons          
// PWM Teensy-Pin 3 controls Servo_1 => Elevator         
// PWM Teensy-Pin 4 controls Servo_2 => Rudder         
// PWM Teensy-Pin 5 Controls Servo_3 => Motor         

//      Serial.print("23: T-Pin 2=> Servo_0=> Ailerons= ");
//      Serial.print(Servo_0_In);
//      Serial.println();
//      Serial.print("23: T-Pin 3=> Servo_1=> Elevator= ");
//      Serial.print(Servo_1_In);
      Serial.println();
      Serial.print("23: T-Pin 4=> Servo_2=> Rudder= ");
      Serial.print(Servo_2_In);
      Serial.println();
//      Serial.print("23  T-Pin 5=> Servo_3=> Motor= ");
//      Serial.print(Servo_3_In);

//      Serial.println();   
     }  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++ End Servo Control Function ++++++++++++++
//+++++++++++++++++++ End Servo Control Function ++++++++++++++
//+++++++++++++++++++ End Servo Control Function ++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++