.......... PID Roll Controller Source Code ...........

========== File-Name: 19_Pid_Roll_Controller.ino ===========

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++ Begin PID-Roll control functionn +++++++++++++++
//             Last Update: 23-08-2022  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void control_pid_roll(float roll_setpoint)
{
  float pid_p_roll_gain = 0.7;  //gain for p rollangle error
  float pid_i_roll_gain = 0.2;  //0.6; gain for i rollangle error
  int   iR_switch = 1;           // is 0 or 1 for testing only 
  float pid_d_roll_gain = 0.0;  //0.5; gain for d rollangle
  
  float pid_p_roll_angle_error;  //, pid_p_roll_output;
  float pid_i_roll_angle_error = 0.0;
  float pid_d_roll_angle_error = 0.0;
  float pid_roll_error_sum;
//  float last_pid_p_roll_angle_error;

  int p_max_roll_angle_error =  30;  // Maximum p-output of roll PID-controller (+/-) deg
  int p_min_roll_angle_error = -30;  // Minimum p-output of roll PID-controller (+/-) deg

//  int i_min_roll_angle_error = -25; // Minimum i-output of roll PID-controller (+/-) deg
//  int i_max_roll_angle_error =  25; // Maximum i-output of roll PID-controller (+/-) deg

  int max_pid_roll_output = 2400;   //800 <=> 2400  max servo stroke
  int min_pid_roll_output = 800;    //1600 is neutrale stand
  

// P: Proportional PID error term in [degrees]
      pid_p_roll_angle_error = GyroRoll - roll_setpoint;
  
      if (pid_p_roll_angle_error > p_max_roll_angle_error) 
          pid_p_roll_angle_error = p_max_roll_angle_error;
      if (pid_p_roll_angle_error < p_min_roll_angle_error) 
          pid_p_roll_angle_error = p_min_roll_angle_error;
    
//      Serial.print("20: Pid_p_roll_angle_error = ");
//      Serial.println(pid_p_roll_angle_error,1);

//..................................................................
// I: Integral PID error term in [degrees/sec]
      pid_i_roll_angle_error += pid_i_roll_gain * pid_p_roll_angle_error; 

//      if(pid_i_roll_angle_error > i_max_roll_angle_error) 
//         pid_i_roll_angle_error = i_max_roll_angle_error;
//      if(pid_i_roll_angle_error < i_min_roll_angle_error)
//         pid_i_roll_angle_error = i_min_roll_angle_error;

//      Serial.print("20: Pid_i_roll_angle_error = ");
//      Serial.println(pid_i_roll_angle_error,1);

/*
//..................................................................
// D: Differential PID error term in [degrees/second]
      pid_d_roll_angle_error = GyroRolldot;    //Time derivative of GyroRoll; 
      // max.gemeten GyroY_rate +/-100 deg/s   

//    pid_d_roll_angle_error = pid_p_roll_angle_error - last_pid_p_roll_angle_error;
//      last_pid_p_roll_angle_error = pid_p_roll_angle_error;

//      Serial.print("19 Pid_d_roll_angle_error = ");
//      Serial.println(pid_d_roll_angle_error,1);
//...................................................................
*/

   pid_roll_error_sum = pid_p_roll_gain * pid_p_roll_angle_error 
                      + pid_i_roll_angle_error * iR_switch
                      + pid_d_roll_gain * pid_d_roll_angle_error;

//   pid_roll_output = mapfloat(pid_roll_error_sum,
//                     p_min_roll_angle_error, p_max_roll_angle_error,
//                     min_pid_roll_output, max_pid_roll_output);

// Below map aligns wooden cube axis with stabilizing Ailerons roll
  pid_roll_output = mapfloat(pid_roll_error_sum,
                    p_min_roll_angle_error, p_max_roll_angle_error,
                    max_pid_roll_output, min_pid_roll_output);

  pid_roll_output = pid_roll_output-70;

      if (pid_roll_output > max_pid_roll_output)
          pid_roll_output = max_pid_roll_output;  //2400
      if (pid_roll_output < min_pid_roll_output)
          pid_roll_output = min_pid_roll_output;  //800 

//   pid_roll_output = Moving_Average (10, pid_roll_output);
//     pid_roll_output = Rotating_Memory(pid_roll_output);

//      Serial.print("20: Pid_roll_output = ");
//      Serial.println(pid_roll_output,0);
     
  }  

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ End PID-Roll control functionn +++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++