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

//---------- Functions -------------------------------
void init_pids ()
{
	right_mtr_pid.SetSampleTime(10);
	right_mtr_pid.SetMode(AUTOMATIC);
	//right_mtr_pid.SetOutputLimits(20,240);   //min, max
	left_mtr_pid.SetSampleTime(10);
	left_mtr_pid.SetMode(AUTOMATIC);
	//left_mtr_pid.SetOutputLimits(20,240);   //min, max

}

 void handle_enabled() {
   if (settings.enabled) {
     reset_window();
     pid.SetMode(AUTOMATIC);
   } else {
     cancel_autotune();
     pid.SetMode(MANUAL);
     set_heat(false);
   }
 }

 void Stop(PID& pid) {
     pid.SetMode(MANUAL);
     SetSpeed(0);
     
     m_fPower = 0.0;
     m_nTicksPID = 0;
     m_nLastCompute = 0;
     
     analogWrite(POWER, 0);
     pid.SetMode(AUTOMATIC);
 }

void setup() {
  // put your setup code here, to run once:
  pinMode(pinHeater, OUTPUT);
  pinMode(pinThermistor, INPUT);
  digitalWrite(pinHeater, 0);

  Serial.begin(57600);
  Serial.setTimeout(250); // Set timeout to 250ms

  // Restore calibration if already set.
  thermCalibrated = true;
  if(EEPROM.read(thermStepSetAddr) == 0) {
    unsigned char setPoint[4];
    for(int i = 0; i < 4; i++)
      setPoint[i] = EEPROM.read(thermStepAddr+i);
    thermStep = *((float*)setPoint);
  } else {
    thermCalibrated = false;
  }
  
  if(EEPROM.read(thermYIntSetAddr) == 0) {
    unsigned char setPoint[4];
    for(int i = 0; i < 4; i++)
      setPoint[i] = EEPROM.read(thermYIntAddr+i);
    thermYInt = *((float*)setPoint);
  } else {
    thermCalibrated = false;
  }

  pid.SetOutputLimits(0.0f, 1.0f);
  pid.SetSampleTime(250); // 250ms compute period
  pid.SetMode(MANUAL);
}

void PIDControl::optionsPID(int setTimePoint)
{
    //tell the PID to range between 0 and the full window size
    myPID.SetOutputLimits(0, setTimePoint);

    //turn the PID on
    myPID.SetMode(AUTOMATIC);
}

void setup()
{
    //initialize the variables we're linked to
    Input = analogRead(0);
    Setpoint = 100;

    //turn the PID on
    myPID.SetMode(AUTO);
}

void motorsInit( int leftPin, int rightPin )
{
  servoLeft.attach(leftPin);
  servoRight.attach(rightPin);
  servoLeft.writeMicroseconds(servoSpeedLeft);
  servoRight.writeMicroseconds(servoSpeedRight);
#ifdef USE_PID
  Input = 0;
  Setpoint = 0;
  turningPID.SetMode(AUTOMATIC);
  turningPID.SetOutputLimits(-SERVO_DRIVE_TURN_SPEED, SERVO_DRIVE_TURN_SPEED);
#endif
}

void setup(void) {
//  save_configs();
  Serial.begin(115200);
  read_configs();
  InitButton();
  LCD.InitLCD();
  pinMode(LCD_LIGHT, OUTPUT); digitalWrite(LCD_LIGHT, LOW);
  pinMode(ALERT_LAMP, OUTPUT);  digitalWrite(ALERT_LAMP, HIGH);
  pinMode(SSR_PIN, OUTPUT);
  SetTemp=cfg.DispTemp;
  myPID.SetMode(AUTOMATIC);
  LCD.drawBitmap(0, 0, arduino_logo, 84, 48); delay(2000);
  LCD.invert(true); delay(500); LCD.invert(false);  delay(500);
  digitalWrite(ALERT_LAMP, LOW);
}

void setup()
{
  Wire.begin(myAddress);
  Wire.onReceive(i2cReceive);
  Wire.onRequest(i2cRequest);

  pinMode(limit_switch_pin, INPUT); 

  mySerial.begin(9600); // Serial commuication to motor controller start.

  setpoint = 0.0;

  calibration(); // Running the calibration code on every start up

  input = encoderRead();

  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-127, 127);
  myPID.SetSampleTime(20);
}

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

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

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

void initIMU(void) {
	float selfTest9250[6];

	/* Initialize motors */
	Motor_Init();
	trace_printf("Motor initialized.\n");

	/* Initialize related variables */
	GyroMeasError = _PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
	beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
	GyroMeasDrift = _PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
	zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value

	/* Initialize balancing */
	bal_pitch = 0;
	bal_roll = 0;
	bal_yaw = 0;

	/* Initialize PID */
	pitchReg.SetMode(AUTOMATIC);
	pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);

	rollReg.SetMode(AUTOMATIC);
	rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);

	yawReg.SetMode(AUTOMATIC);
	yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);
	trace_printf("PID initialized.\n");

	/* Detect whether MPU9250 is online */
	MPU9250_I2C_Init();
	resetMPU9250();

	if(MPU9250_I2C_ByteRead(MPU9250_ADDRESS, WHO_AM_I_MPU9250) != 0x71) {
		trace_printf("Could not find MPU9250!\n");
	}
	trace_printf("MPU9250 is online.\n");
	timer_sleep(100);

	/* MPU9250 self test and calibrate */
	MPU9250SelfTest(selfTest9250);
	trace_printf("x-axis self test: acceleration trim within : %6f %% of factory value\n", selfTest9250[0]);
	trace_printf("y-axis self test: acceleration trim within : %f %% of factory value\n", selfTest9250[1]);
	trace_printf("z-axis self test: acceleration trim within : %f %% of factory value\n", selfTest9250[2]);
	trace_printf("x-axis self test: gyration trim within : %f %% of factory value\n", selfTest9250[3]);
	trace_printf("y-axis self test: gyration trim within : %f %% of factory value\n", selfTest9250[4]);
	trace_printf("z-axis self test: gyration trim within : %f %% of factory value\n", selfTest9250[5]);
	timer_sleep(100);
	calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
	trace_printf("x gyro bias = %f\n", gyroBias[0]);
	trace_printf("y gyro bias = %f\n", gyroBias[1]);
	trace_printf("z gyro bias = %f\n", gyroBias[2]);
	trace_printf("x accel bias = %f\n", accelBias[0]);
	trace_printf("y accel bias = %f\n", accelBias[1]);
	trace_printf("z accel bias = %f\n", accelBias[2]);
	timer_sleep(100);

	/* Initialize MPU9250 and AK8963 */
	initMPU9250();
	trace_printf("MPU9250 initialized for active data mode....\n"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
	timer_sleep(500);
    initAK8963(magCalibration);
    trace_printf("AK8963 initialized for active data mode....\n"); // Initialize device for active mode read of magnetometer
    trace_printf("Accelerometer full-scale range = %f  g\n", 2.0f*(float)(1<<Ascale));
    trace_printf("Gyroscope full-scale range = %f  deg/s\n", 250.0f*(float)(1<<Gscale));
    if(Mscale == 0) trace_printf("Magnetometer resolution = 14  bits\n");
    if(Mscale == 1) trace_printf("Magnetometer resolution = 16  bits\n");
    if(Mmode == 2) trace_printf("Magnetometer ODR = 8 Hz\n");
    if(Mmode == 6) trace_printf("Magnetometer ODR = 100 Hz\n");
    timer_sleep(100);
    getAres(); // Get accelerometer sensitivity
    getGres(); // Get gyro sensitivity
    getMres(); // Get magnetometer sensitivity
    trace_printf("Accelerometer sensitivity is %f LSB/g \n", 1.0f/aRes);
    trace_printf("Gyroscope sensitivity is %f LSB/deg/s \n", 1.0f/gRes);
    trace_printf("Magnetometer sensitivity is %f LSB/G \n", 1.0f/mRes);
    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
}

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