C++ PID::Compute方法代码示例

void computePID(void) {
	  if((pitch_set < PITCH_MIN) || (pitch_set > PITCH_MAX)) pitch_set = pitch_set_last;
	  if((roll_set < ROLL_MIN) || (roll_set > ROLL_MAX)) roll_set = roll_set_last;
	  if((yaw_set < YAW_MIN) || (yaw_set > YAW_MAX)) yaw_set = yaw_set_last;

	  pitch_set_last = pitch_set;
	  roll_set_last = roll_set;
	  pitch_set_last = pitch_set;

	  //////////////////////////////////////////////////////
	  //////////////////////////////////////////////////////
	  // FOR TEST ONLY /////////////////////////////////////
	  //////////////////////////////////////////////////////
	  //////////////////////////////////////////////////////
	  ypr[0] = 0;

	  if(abs(ypr[0]-yprLast[0])>30) ypr[0] = yprLast[0];
	  if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
	  if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];

	  yprLast[0] = ypr[0];
	  yprLast[1] = ypr[1];
	  yprLast[2] = ypr[2];

	  pitchReg.Compute();
	  rollReg.Compute();
	  yawReg.Compute();

	  //trace_printf("yprPID: %f, %f, %f\n", bal_yaw, bal_pitch, bal_roll);
}

void loop()
{
  input = encoderRead();

  if(new_kP != old_kP)
  {
      myPID.SetTunings(new_kP,0,0);
      old_kP = new_kP;
  }

  myPID.Compute();
  double motorInput;
  if (output < -1)
  {
    motorInput = (112.0/127.0)*output -15;
  }
  else if (output > 1)
  {
    motorInput = (112.0/127.0)*output +15;
  }
  else
  {
    motorInput = output;
  }

  motorInput = motorInput+127.0;

  mySerial.write((int)motorInput);
  delay(120);
}

 bool ComputePID(PID& pid) {
     unsigned long nNow = millis();
     unsigned long nChange = nNow - m_nLastCompute;
     m_fTicksPerSecond = m_nTicksPID / (nChange / 1000.0);
     if(pid.Compute()) { // is the sample time low enough?
         m_nTicksPID = 0;
         m_nLastCompute = nNow;
         analogWrite(POWER, (int)m_fPower);
         return true;
     }
     return false;
 }

void loop(void) {
	SetTemp=cfg.DispTemp;
	SetHumi=cfg.DispHumi;
	float ndo = read_ds(&ds_stt);
	if(ndo!=9999) NowTemp=ndo;
	read_dht(&dht_temp,&dht_humi);
	double gap = abs(SetTemp-NowTemp); //distance away from setpoint
	if (gap < 10){  //we're close to setpoint, use conservative tuning parameters
		myPID.SetTunings(consKp, consKi, consKd);
	}else{	//we're far from setpoint, use aggressive tuning parameters
		myPID.SetTunings(aggKp, aggKi, aggKd);
	}
	myPID.Compute();
	analogWrite(SSR_PIN, (byte)OutPWM);
	if(dht_humi<SetHumi){	digitalWrite(HUMI_GENERATE, HIGH);}else{		digitalWrite(HUMI_GENERATE, LOW);}
	Display();
}

void control_loop() {
  pinMode( control_pin, OUTPUT );
  
  if(tuning && auto_tune.Runtime()) {
    finish_autotune();
  } else if(!tuning) {
    pid.SetMode(!config.paused);
    pid.SetTunings(profiles[config.driving].kp, profiles[config.driving].ki, profiles[config.driving].kd);
    pid.SetSampleTime(1000 * profiles[config.driving].sample_time);  // Update the control value once per second
    current_target_temp = profiles[config.driving].target_temp;
    pid.Compute();
  }
  
  if(last_power != power) {
    last_power = power;
    analogWrite(control_pin, power);
    Serial.print("Log\tpower\t");
    Serial.println((100.0 * ((float)power / 255.0)));
  }
}

double PIDControl::reflowPID(double setTempPoint, double setTimePoint, unsigned long windowStartTime)
{
    _val=analogRead(sensePin);
    currTemp = _val * 0.00489 / 0.005;	//Converting the voltage of sensePin to current temp.

    displayTemp(currTemp,setTempPoint);

    myPID.Compute();
    Serial.println(Output);
    return Output;

    _now = millis();
    if((_now - windowStartTime)/1000 > setTimePoint)
    {   //time to shift the Reflow Window
        windowStartTime += setTimePoint;
    }
    if (Output > (_now - windowStartTime)/1000)
    {
        digitalWrite(heaterPin,HIGH);
    }
    else digitalWrite(heaterPin,LOW);
}

  void update() {
    if (settings.enabled) {
      if (autotune_running) {
        if (pid_autotune.Runtime()) {
          autotune_running = false;

          settings.pid.kp = pid_autotune.GetKp();
          settings.pid.ki = pid_autotune.GetKi();
          settings.pid.kd = pid_autotune.GetKd();

          update_pid_tunings();
          write_settings();
        }
      }

      if (! autotune_running) {
        pid.Compute();
      }

      pid_output_sum += pid_output;
      pid_output_count++;

      unsigned long now = millis();
      if (now >= window_end_millis) {
        pid_output_average = pid_output_sum / pid_output_count;

        if (pid_output_average >= settings.heat_on.threshold) {
          set_heat(true);
        } else if (pid_output_average + settings.heat_on.delta <=
                   settings.heat_on.threshold) {
          set_heat(false);
        }

        reset_window();
      }
    }
  }

void loop() {
  // put your main code here, to run repeatedly:
  String str = Serial.readStringUntil('\n');
  const char *cmd = str.c_str();
  if(strncmp(cmd, "AT+ SetTemp", 11) == 0 && strlen(cmd) > 13) {
    const char *target = cmd + 12;
    targetTemp = (float)atoi(target) / 100.0f;
  
    // Don't allow 100C to be exceeded.
    if(targetTemp > 100.0f) targetTemp = 100.0f;
  
    // Convert to Kelvin
    targetTemp += 273.15;

    // PID SetMode method ignores if we go from automatic
    // to automatic
    pid.SetMode(AUTOMATIC);
      
    Serial.print("AT- SetTempOk\r\n");
  } else if(strncmp(cmd, "AT+ GetActualTemp", 17) == 0) {
    char buf[64];
    snprintf(buf, 64, "AT- ActualTemp %u\r\n", (unsigned int)((actualTemp-273.15) * 100));
    Serial.print(buf);
  } else if(strncmp(cmd, "AT+ GetTargetTemp", 17) == 0) {
    char buf[64];
    snprintf(buf, 64, "AT- TargetTemp %u\r\n", (unsigned int)((targetTemp-273.15) * 100));
    Serial.print(buf);
  } else if(strncmp(cmd, "AT+ TurnOff", 11) == 0) {
    pid.SetMode(MANUAL);
    pulseWidth = 0;
    Serial.print("AT- TurnOffOk\r\n");
  } else if(str.length() > 0) {
    Serial.print("AT- UnknownCmd\r\n");
  }

  // Get actual temp from thermistor
  // Using Steinhart-Hart. Based on
  // http://playground.arduino.cc/ComponentLib/Thermistor2
  int pinValue = analogRead(pinThermistor);
  //float v1 = (float)pinValue / 1024.0f * supplyVoltage;
  // Simple voltage divider.
  // v1 = supplyVoltage * Rt / (balanceResistor + Rt)
  // Solve for Rt.
  float rVal = balanceResistor * (1023.0f/pinValue-1);
  float lnR = log(rVal);
  float tinv = tA + tB * lnR + tC * lnR * lnR * lnR;
  actualTemp = 1.0f / tinv;

  if(fabs(actualTemp - targetTemp) > 5) {
    pid.SetTunings(kPfar, kIfar, kDfar);
  } else {
    pid.SetTunings(kPnear, kInear, kDnear);
  }

  pid.Compute();
  
  // TODO: not sure if analogWrite will work correctly,
  // may turn heater pad on/off too quickly or relay may be 
  // too slow.
  analogWrite(pinHeater, pulseWidth * 255);
}

/* Calcule les pwm a appliquer pour un asservissement en vitesse en trapeze
 * <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
 * <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
 * */
void speedControl(int* value_pwm_left, int* value_pwm_right){
	/* si le robot est en train de tourner, et qu'on lui donne une consigne de vitesse, il ne va pas partir droit
	 * solution = decomposer l'asservissement en vitesse en 2 :
	 * -> stopper le robot (les 2 vitesses = 0)
	 * -> lancer l'asservissement en vitesse
	 */

	static int start_time;

	static bool initDone = false;

	if(!initDone){
		start_time = 0;
		pwm = 0;
		currentSpeed = 0;
		consigne = 0;
		pid4SpeedControl.Reset();
		pid4SpeedControl.SetInputLimits(-20000,20000);
		pid4SpeedControl.SetOutputLimits(-255,255);
		pid4SpeedControl.SetSampleTime(DUREE_CYCLE);
		pid4SpeedControl.SetMode(AUTO);
		initDone = true;
	}

	/* Gestion de l'arret d'urgence */
	if(current_goal.isCanceled){
		initDone = false;
		current_goal.isReached = true;
		current_goal.isCanceled = false;
		/* et juste pour etre sur */
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
		return;
	}

	/* Gestion de la pause */
	if(current_goal.isPaused){
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
		return;
	}

	if(current_goal.phase == PHASE_1){ //phase d'acceleration
		consigne = current_goal.speed;
		currentSpeed = robot_state.speed;
		if(abs(consigne-currentSpeed) < 1000){ /*si l'erreur est inferieur a 1, on concidere la consigne atteinte*/
			current_goal.phase = PHASE_2;
			start_time = millis();
		}
	}
	else if(current_goal.phase == PHASE_2){ //phase de regime permanent
		consigne = current_goal.speed;
		currentSpeed = robot_state.speed;
		if(millis()-start_time > current_goal.period){ /* fin de regime permanent */
			current_goal.phase = PHASE_3;
		}
	}
	else if(current_goal.phase == PHASE_3){ //phase de decceleration
		consigne = 0;
		currentSpeed = robot_state.speed;
		if(abs(robot_state.speed)<1000){
			current_goal.phase = PHASE_4;
		}
	}

	pid4SpeedControl.Compute();

	if(current_goal.phase == PHASE_4){
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
		current_goal.isReached = true;
		initDone = false;
	}else{
		(*value_pwm_right) = pwm;
		(*value_pwm_left) = pwm;
	}
}

//.........这里部分代码省略.........
	Serial.print(angularCoeff);
	Serial.print("  angle:");
	Serial.print(robot_state.angle);
	Serial.print("  alpha:");
	Serial.print(currentAlpha);
	Serial.print("  delta:");
	Serial.print(currentDelta);
	Serial.print("  x:");
	Serial.print(current_goal.x);
	Serial.print("  y:");
	Serial.println(current_goal.y);
	*/
	
	
	
	/* on limite la vitesse lineaire quand on s'approche du but */
	if (abs(currentDelta)<250){
		pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
		pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
	}
	else if (abs(currentDelta)<500){
		pid4DeltaControl.SetOutputLimits(-min(60,current_goal.speed),min(60,current_goal.speed)); // composante liee a la vitesse lineaire
		pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
	}
	else if (abs(currentDelta)<750){
		pid4DeltaControl.SetOutputLimits(-min(80,current_goal.speed),min(80,current_goal.speed)); // composante liee a la vitesse lineaire
		pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
	}
	else if (abs(currentDelta)<1000){
		pid4DeltaControl.SetOutputLimits(-min(100,current_goal.speed),min(100,current_goal.speed)); // composante liee a la vitesse lineaire
		pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
	}
	else if (abs(currentDelta)<1250){
		pid4DeltaControl.SetOutputLimits(-min(150,current_goal.speed),min(150,current_goal.speed)); // composante liee a la vitesse lineaire
		pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
	}
	else if (abs(currentDelta)<1500){
		pid4DeltaControl.SetOutputLimits(-min(200,current_goal.speed),min(200,current_goal.speed)); // composante liee a la vitesse lineaire
		pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
	}

	if(abs(currentDelta) < 5*ENC_MM_TO_TICKS) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
		current_goal.phase = PHASE_2;
	else
		current_goal.phase = PHASE_1;

	pid4AlphaControl.Compute();
	pid4DeltaControl.Compute();

	if(current_goal.phase == PHASE_2){
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
	}
	else{
		double pwm4Delta = 0.0;

		//FIXME probleme de debordemment
		//alpha 200 delta 255 / delta+alpha = 455 / delta-alpha = 55 / => alpha 200 delta 55
		//alpha 200 delta -255 / delta+alpha = -55 / delta-alpha = -455 / => alpha 200 delta -55
		//alpha -200 delta 255 / delta+alpha = 55 / delta-alpha = 455 / => alpha -200 delta 55
		//alpha -200 delta -255 / delta+alpha = -455 / delta-alpha = -55 / => alpha -200 delta -55
		/*
		 Correction by thomas
		 if(output4Delta+output4Alpha>255 || output4Delta-output4Alpha>255)
			pwm4Delta = 255-output4Alpha;
		else if(output4Delta+output4Alpha<-255 || output4Delta-output4Alpha<-255)
			pwm4Delta = -255+output4Alpha;
		else
			pwm4Delta = output4Delta;*/

		(*value_pwm_right) = output4Delta+output4Alpha;
		(*value_pwm_left) = output4Delta-output4Alpha;
		
		// Correction by thomas
		if ((*value_pwm_right) > 255)
			(*value_pwm_right) = 255;
		else if ((*value_pwm_right) < -255)
			(*value_pwm_right) = -255;
		
		if ((*value_pwm_left) > 255)
			(*value_pwm_left) = 255;
		else if ((*value_pwm_left) < -255)
			(*value_pwm_left) = -255;
	}

	if(current_goal.phase == PHASE_2){
		if(current_goal.id != -1 && !current_goal.isMessageSent){
			//le message d'arrivee n'a pas encore ete envoye a l'intelligence
			//envoi du message
			sendMessage(current_goal.id,2);
			current_goal.isMessageSent = true;
		}
		/*condition d'arret = si on a atteint le but et qu'un nouveau but attends dans la fifo*/
		if(!fifoIsEmpty()){ //on passe a la tache suivante
			current_goal.isReached = true;
			initDone = false;
		}
	}

}

/* Calcule les pwm a appliquer pour un asservissement en angle
 * <> value_pwm_left : la pwm a appliquer sur la roue gauche [-255,255]
 * <> value_pwm_right : la pwm a appliquer sur la roue droite [-255,255]
 * */
void angleControl(int* value_pwm_left, int* value_pwm_right){

	static bool initDone = false;

	if(!initDone){
		pwm = 0;
		currentEcart = .0;
		consigne = .0;
		pid4AngleControl.Reset();
		pid4AngleControl.SetInputLimits(-M_PI,M_PI);
		pid4AngleControl.SetOutputLimits(-current_goal.speed,current_goal.speed);
		pid4AngleControl.SetSampleTime(DUREE_CYCLE);
		pid4AngleControl.SetMode(AUTO);
		initDone = true;
	}

	/* Gestion de l'arret d'urgence */
	if(current_goal.isCanceled){
		initDone = false;
		current_goal.isReached = true;
		current_goal.isCanceled = false;
		/* et juste pour etre sur */
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
		return;
	}

	/* Gestion de la pause */
	if(current_goal.isPaused){
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
		return;
	}

	/*l'angle consigne doit etre comprise entre ]-PI, PI]

	En fait pour simplifier, l'entree du PID sera l'ecart entre le l'angle courant et l'angle cible (consigne - angle courant)
	la consigne (SetPoint) du PID sera 0
	la sortie du PID sera le double pwm
	*/
	currentEcart = -moduloPI(current_goal.angle - robot_state.angle);

	/*
	Serial.print("goal: ");
	Serial.print(current_goal.angle);
	Serial.print(" current: ");
	Serial.print(robot_state.angle);
	Serial.print(" ecart: ");
	Serial.println(currentEcart);
	*/

	if(abs(currentEcart) < 3.0f*M_PI/360.0f) /*si l'erreur est inferieur a 3deg, on concidere la consigne atteinte*/
		current_goal.phase = PHASE_2;
	else
		current_goal.phase = PHASE_1;

	pid4AngleControl.Compute();

	if(current_goal.phase == PHASE_2){
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
	}
	else{
		(*value_pwm_right) = pwm;
		(*value_pwm_left) = -pwm;
	}

	if(current_goal.phase == PHASE_2){
		if(current_goal.id != -1 && !current_goal.isMessageSent){
			//le message d'arrivee n'a pas encore ete envoye a l'intelligence
			//envoi du message
			sendMessage(current_goal.id,2);
			current_goal.isMessageSent = true;
		}
		if(!fifoIsEmpty()){ //on passe a la tache suivante
			/*condition d'arret = si on a atteint le but et qu'un nouveau but attends dans la fifo*/
			current_goal.isReached = true;
			initDone = false;
		}
	}
}

//.........这里部分代码省略.........
	

	switch(current_goal.phase)
	{
		case PHASE_1:
			if(abs(currentDelta)<1500) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
			{
				current_goal.phase = PHASE_DECEL;
			}
		break;
		
		case PHASE_DECEL:
			if (fifoIsEmpty())
			{
				/* on limite la vitesse lineaire quand on s'approche du but */
				if (abs(currentDelta)<250){
					pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
					pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
				}
				else if (abs(currentDelta)<500){
					pid4DeltaControl.SetOutputLimits(-min(60,current_goal.speed),min(60,current_goal.speed)); // composante liee a la vitesse lineaire
					pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
				}
				else if (abs(currentDelta)<750){
					pid4DeltaControl.SetOutputLimits(-min(80,current_goal.speed),min(80,current_goal.speed)); // composante liee a la vitesse lineaire
					pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
				}
				else if (abs(currentDelta)<1000){
					pid4DeltaControl.SetOutputLimits(-min(100,current_goal.speed),min(100,current_goal.speed)); // composante liee a la vitesse lineaire
					pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
				}
				else if (abs(currentDelta)<1250){
					pid4DeltaControl.SetOutputLimits(-min(150,current_goal.speed),min(150,current_goal.speed)); // composante liee a la vitesse lineaire
					pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
				}
				else {
					pid4DeltaControl.SetOutputLimits(-min(200,current_goal.speed),min(200,current_goal.speed)); // composante liee a la vitesse lineaire
					pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
				}
			}
			if(abs(currentDelta) < 5*ENC_MM_TO_TICKS) /*si l'ecart n'est plus que de 6 mm, on considere la consigne comme atteinte*/
			{
				//envoi du message
				sendMessage(current_goal.id,2);
				current_goal.isMessageSent = true;
				
				current_goal.phase = PHASE_ARRET;
			}
		break;

		case PHASE_ARRET:
			if (abs(currentDelta) > 5*ENC_MM_TO_TICKS)
			{
				current_goal.phase = PHASE_CORRECTION;
			}
		break;

		case PHASE_CORRECTION:
			pid4DeltaControl.SetOutputLimits(-min(50,current_goal.speed),min(50,current_goal.speed)); // composante liee a la vitesse lineaire
			pid4AlphaControl.SetOutputLimits(-150,150); // composante liee a la vitesse de rotation
			if (abs(currentDelta) < 5*ENC_MM_TO_TICKS)
			{
				current_goal.phase = PHASE_ARRET;
			}
		default:
		
		break;
	}

	if (!fifoIsEmpty() and (current_goal.phase == PHASE_ARRET or current_goal.phase == PHASE_CORRECTION)) { //on passe a la tache suivante si la fifo n'est pas vide
		current_goal.isReached = true;
		initDone = false;
	}
		

	pid4AlphaControl.Compute();
	pid4DeltaControl.Compute();

	if (current_goal.phase == PHASE_ARRET)
	{
		(*value_pwm_right) = 0;
		(*value_pwm_left) = 0;
	}
	else
	{
		(*value_pwm_right) = output4Delta+output4Alpha;
		(*value_pwm_left) = output4Delta-output4Alpha;
		
		// Débordement
		if ((*value_pwm_right) > 255)
			(*value_pwm_right) = 255;
		else if ((*value_pwm_right) < -255)
			(*value_pwm_right) = -255;
		
		if ((*value_pwm_left) > 255)
			(*value_pwm_left) = 255;
		else if ((*value_pwm_left) < -255)
			(*value_pwm_left) = -255;
	}
}

void loop()
{
    Input = analogRead(0);
    myPID.Compute();
    analogWrite(3,Output);
}

int main(int argc, char **argv) {
	START_EASYLOGGINGPP(argc, argv);
    // Load configuration from file
    el::Configurations conf("/home/debian/hackerboat/embedded_software/unified/setup/log.conf");
    // Actually reconfigure all loggers instead
    el::Loggers::reconfigureAllLoggers(conf);
    
    BoatState *me = new BoatState();
	me->rudder = new Servo();
	me->throttle = new Throttle();
	me->orient = new OrientationInput(SensorOrientation::SENSOR_AXIS_Z_UP);
    
	double targetHeading = 0;
	double in = 0, out = 0, setpoint = 0;
	Pin enable(Conf::get()->servoEnbPort(), Conf::get()->servoEnbPin(), true, true);
	PID *helm = new PID(&in, &out, &setpoint, 0, 0, 0, 0);
	helm->SetMode(AUTOMATIC);
	helm->SetControllerDirection(Conf::get()->rudderDir());
	helm->SetSampleTime(Conf::get()->rudderPeriod());
	helm->SetOutputLimits(Conf::get()->rudderMin(), 
						Conf::get()->rudderMax());
	helm->SetTunings(10,0,0);
	
	if (!me->rudder->attach(Conf::get()->rudderPort(), Conf::get()->rudderPin())) {
		std::cout << "Rudder failed to attach 1" << std::endl;
		return -1;
	}
	if (!me->rudder->isAttached())  {
		std::cout << "Rudder failed to attach 2" << std::endl;
		return -1;
	}
	if (me->orient->begin() && me->orient->isValid()) {
		cout << "Initialization successful" << endl;
		cout << "Oriented with Z axis up" << endl;
	} else {
		cout << "Initialization failed; exiting" << endl;
		return -1;
	}
	
	for (int i = 0; i < 100; i++) {
		double currentheading = me->orient->getOrientation()->makeTrue().heading;
		if (isfinite(currentheading)) targetHeading += currentheading;
		cout << ".";
		std::this_thread::sleep_for(100ms);
	}
	cout << endl;
	targetHeading = targetHeading/100;
	cout << "Target heading is " << to_string(targetHeading) << " degrees true " << endl;
	int count = 0;
	for (;;) {
		in = me->orient->getOrientation()->makeTrue().headingError(targetHeading);
		count++;
		LOG_EVERY_N(10, DEBUG) << "True Heading: " << me->orient->getOrientation()->makeTrue() 
								<< ", Target Course: " << targetHeading;
		helm->Compute();
		me->rudder->write(out);
		LOG_EVERY_N(10, DEBUG) << "Rudder command: " << to_string(out);
		std::this_thread::sleep_for(100ms);
		if (count > 9) {
			count = 0;
			cout << "True Heading: " << me->orient->getOrientation()->makeTrue().heading 
								<< "\tTarget Course: " << targetHeading
								<< "\tRudder command: " << to_string(out) << endl;
		}
	}
	
	return 0;
}

	virtual void run() {
		int16_t accData[3];
		int16_t gyrData[3];

		CTimer tm(TIMER0);
		tm.second(DT);
		tm.enable();

		myPID.SetMode(AUTOMATIC);
		myPID.SetOutputLimits(-100.0, 100.0);
		myPID.SetSampleTime(PID_SAMPLE_RATE);

		m_roll = 0.0f;
		m_pitch = 0.0f;

		double output;

#if USE_AUTO_TUNING
		double sp_input, sp_output, sp_setpoint, lastRoll;
		PID speedPID(&sp_input, &sp_output, &sp_setpoint, 35, 1, 2, DIRECT);
		speedPID.SetMode(AUTOMATIC);
		speedPID.SetOutputLimits(-config.roll_offset, config.roll_offset);
		speedPID.SetSampleTime(PID_SAMPLE_RATE);
		sp_input = 0;
		sp_setpoint = 0;
		lastRoll = 0;
#endif
		//
		// loop
		//
		while (isAlive()) {

			//
			// wait timer interrupt (DT)
			//
			if (tm.wait()) {
				//
				// read sensors
				//
				m_mxMPU.lock();
				m_mpu->getMotion6(&accData[0], &accData[1], &accData[2],
						&gyrData[0], &gyrData[1], &gyrData[2]);
				m_mxMPU.unlock();

				//
				// filter
				//
				ComplementaryFilter(accData, gyrData, &m_pitch, &m_roll);


#if USE_AUTO_TUNING
				sp_input = (lastRoll - m_roll) / PID_SAMPLE_RATE;	// roll speed
				lastRoll = m_roll;
				speedPID.Compute();
				m_setpoint = -sp_output;							// tune output angle
#else
				m_roll -= config.roll_offset;
#endif
				m_input = m_roll;
				myPID.Compute();

				if ( m_output > config.skip_interval ) {
					LEDs[1] = LED_ON;
					LEDs[2] = LED_OFF;
					output = map(m_output, config.skip_interval, 100, config.pwm_min, config.pwm_max);

					m_left->direct(DIR_FORWARD);
					m_right->direct(DIR_FORWARD);

				} else if ( m_output<-config.skip_interval ) {
					LEDs[1] = LED_OFF;
					LEDs[2] = LED_ON;
					output = map(m_output, -config.skip_interval, -100, config.pwm_min, config.pwm_max);

					m_left->direct(DIR_REVERSE);
					m_right->direct(DIR_REVERSE);
				} else {
					LEDs[1] = LED_OFF;
					LEDs[2] = LED_OFF;
					output = 0;
					m_left->direct(DIR_STOP);
					m_right->direct(DIR_STOP);
				}

				//
				// auto power off when fell.
				//
				if ( abs(m_roll)>65 ) {
					output = 0;
				}

				//
				// output
				//
				m_left->dutyCycle(output * config.left_power);
				m_right->dutyCycle(output * config.right_power);

			}
		}