controls.pde

Go to the documentation of this file.

/***********************************************************
 * StoRC - Search to Rescue Craft
 * MEAM Senior Design 2011-2012
 * 
 * Authors:  Michael Posner (CIS, MEAM '12)
 * Timothy Hennelly (ESE '12)
 * Jacob Orloff (MEAM '12)
 * 
 * (some code from ArduPilot Mega project, found here:
 * http://code.google.com/p/ardupilot-mega/ )
 * 
 * Licensing:
 * 
 * This program is free software: you can redistribute it and/or modify 
 * it under the terms of the GNU General Public License as published by 
 * the Free Software Foundation, either version 3 of the License, or 
 * (at your option) any later version. 
 * 
 * This program is distributed in the hope that it will be useful, 
 * but WITHOUT ANY WARRANTY; without even the implied warranty of 
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
 * GNU General Public License for more details. 
 * 
 * You should have received a copy of the GNU General Public License 
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 * 
 ***********************************************************/

/**
 *  @file controls.pde
 *  @brief Contains all controls functions for feedback and inner loop control
 *  @author Michael Posner (CIS, MEAM '12)
 *  @author Timothy Hennelly (ESE '12)
 *  @author Jacob Orloff (MEAM '12)
 *  
 */

/**
 * PID control on roll angle
 * @param dt the time elapsed since the last call (for integration)
 */

void THR_control(float dt) {

  //calculate gain multiplier based on rc_6 & rc_7
  kp_thr_gm = (float)(rc_6-1000)/1000.0f * 3.0;  // Proportional gain
  ki_thr_gm = (float)(rc_7-1000)/1000.0f * 3.0;  // Integral gain
  
  del_ALT = new_ALT - old_ALT;

  I_THR += KI_THR * ki_thr_gm * del_ALT;

  I_THR = constrain(I_THR,-200,200);

  del_THR = -(KP_THR * kp_thr_gm * (del_ALT/dt) + I_THR);

  //round to nearest 20 to make servos happy
  del_THR = (int)del_THR / 10;
  del_THR = (int)del_THR * 20;

  Serial.print(" rc_thr_trim: ");
  Serial.print(rc_thr_trim);
  Serial.print(" del_ALT: ");
  Serial.print(del_ALT);
  Serial.print(" I_THR: ");
  Serial.print(I_THR);
  Serial.print(" P+D: ");
  Serial.print(del_THR-I_THR);
  Serial.print(" Total: ");
  Serial.println(del_THR);

  //can't affect the throttle too much
  del_THR = constrain(del_THR,-400,400);
  
  old_ALT = new_ALT;
  
} 

void RUD_control(float dt) {

  /* Keeping GMs constant for now
  if(rc_4 < 1500){  // only change rudder GM's when we're not tweaking throttle GM's (levels of priority)
    //calculate gain multiplier based on rc_6 & rc_7
    kp_rud_gm = (float)(rc_6-1000)/1000.0f * 3.0;  // Proportional gain
    ki_rud_gm = (float)(rc_7-1000)/1000.0f * 3.0;  // Integral gain
  }
  */

  I_RUD += KI_RUD * ki_rud_gm * (int)(phi_c - dcm.roll_sensor/100.0) * dt;

  I_RUD = constrain(I_RUD,-150,150);

  del_RUD = KD_RUD * gyro.x + KP_RUD * kp_rud_gm * (int)(phi_c - dcm.roll_sensor/100.0) + I_RUD;

  //round to nearest 20 to make servos happy
  del_RUD = (int)del_RUD / 10;
  del_RUD = (int)del_RUD * 20;

  if(rc_4 < 1500){
    /*
    Serial.print(" rc_rud_trim: ");
    Serial.print(rc_rud_trim);
    Serial.print(" Phi_c: ");
    Serial.print(phi_c);
    Serial.print(" Phi: ");
    Serial.print(dcm.roll_sensor/100.0);
    Serial.print(" Error: ");
    Serial.print(phi_c-dcm.roll_sensor/100.0);
    Serial.print(" I_RUD: ");
    Serial.print(I_RUD);
    Serial.print(" P+D: ");
    Serial.print(del_RUD-I_RUD);
    Serial.print(" Total: ");
    Serial.println(del_RUD);
    */
  }

  //can't affect the rudder too much
  del_RUD = constrain(del_RUD,-400,400);
}

/**
 * Add elevator
 */
 
/*
void ELE_control(float dt){

  q = gyro.y;
  q_filtered = low_pass(q, alpha_q, q_filtered);

  kp_ele_gm = (float)(rc_6-1000)/1000.0f * 3.0;  // Proportional gain for elevator angle
  ki_ele_gm = (float)(rc_7-1000)/1000.0f * 3.0;

  I_ELE += KI_ELE * ki_ele_gm * (q_c - q_filtered)*dt;

  I_ELE = constrain(I_ELE,-100,100);

  del_ELE = KP_ELE * kp_ele_gm * (q_c - q_filtered) + I_ELE;

  del_ELE  = (int)del_ELE  / 10;
  del_ELE  = (int)del_ELE  * 20;

  del_ELE = constrain(del_ELE,-400,400);

  Serial.print(" rc_ele_trim: ");
  Serial.print(rc_ele_trim);
  Serial.print(" Pitch rate command: ");
  Serial.print(q_c);
  Serial.print(" Pitch rate: ");
  Serial.print(q);
  Serial.print(" Filtered: ");
  Serial.print(q_filtered);
  Serial.print(" Error: ");
  Serial.print(q_c-q_filtered);
  Serial.print(" I_ELE: ");
  Serial.print(I_ELE);
  Serial.print(" P+D: ");
  Serial.print(del_ELE - I_ELE);
  Serial.print(" Total: ");
  Serial.println(del_ELE);
}
*/

/**
 * Add rudder based on roll (bank) angle (phi).
 */

/**
 * Add rudder based on current throttle position to offset the additional motor
 * torque caused by an increase in throttle.
 */
void throttle_mixing(void) {

  //throttle_mix_result = (rc_throttle - 1000) / 100 * 10;
  //throttle_mix_result = throttle_mix_result * 2;

  throttle_mix_result = (int)( ((float)(rc_throttle-1000))/1000.0f * 0.1f*1000.0f );  //limit it to 10% of RC capability

  //round values to nearest 20 so the servos don't freak out
  //  because they may not be able to reach a precise value
  throttle_mix_result = throttle_mix_result / 10;
  throttle_mix_result = throttle_mix_result * 20;
  /*
  if(slide_sw == HIGH) { //only if the slide switch is high
   rc_rudder += throttle_mix_result;
   }
   */
}


/**
 * Perturbation routine that periodically adds step inputs to rudder,
 * elevator, etc.  Data collected from the results can be used to validate
 * the simulation model.
 */
void perturb(void) {

  if(slide_sw == HIGH){  //only if the slide switch is high

    if( millis() - ptimer > 1*60000 ) { //if a minute has passed since the last perturbation
      rc_elevator += 300;
    }
    else if(millis() - ptimer > 1*60000+100) {  //add it for 100 ms, then return to normal
      ptimer = millis();
    }

  } 
}