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

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);
}

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 onSetAnglePID()
{
    Serial.println("Receiving Angle tuning command...");
    float Kp, Ki, Kd;
    readTuningArgs(&Kp, &Ki, &Kd);
    anglePID.SetTunings(Kp, Ki, Kd);
    cmdMessenger.sendCmd(kStatus, "Angle PID tuned");
}

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)));
  }
}

 void update_pid_tunings() {
   pid.SetTunings(settings.pid.kp, settings.pid.ki, settings.pid.kd);
 }

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);
}

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;
}