C++ PID类代码示例

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

bool GMEventManager::AddNewGMEvent (Client* client, csString eventName, csString eventDescription)
{
    int newEventID, zero=0;
    PID gmID = client->GetPID();
    int clientnum = client->GetClientNum();
    GMEvent* gmEvent;

    // if this GM already has an active event, he/she cant start another
    if ((gmEvent = GetGMEventByGM(gmID, RUNNING, zero)) != NULL)
    {
        psserver->SendSystemInfo(clientnum,
                                 "You are already running the \'%s\' event.",
                                 gmEvent->eventName.GetDataSafe());
        return false;
    }

    if (eventName.Length() > MAX_EVENT_NAME_LENGTH)
    {
        eventName.Truncate(MAX_EVENT_NAME_LENGTH);
        psserver->SendSystemInfo(client->GetClientNum(),
                                 "Event name truncated to \'%s\'.",
                                 eventName.GetDataSafe());
    }


    // GM can register his/her event
    newEventID = GetNextEventID();

    // remove undesirable characters from the name & description
    csString escEventName, escEventDescription;
    db->Escape(escEventName, eventName.GetDataSafe());
    db->Escape(escEventDescription, eventDescription.GetDataSafe());

    // first in the database
    db->Command("INSERT INTO gm_events(id, gm_id, name, description, status) VALUES (%i, %i, '%s', '%s', %i)",
        newEventID,
        gmID.Unbox(),
        escEventName.GetDataSafe(),
        escEventDescription.GetDataSafe(),
        RUNNING);

    // and in the local cache.
    gmEvent = new GMEvent;
    gmEvent->id = newEventID;
    gmEvent->gmID = gmID;
    gmEvent->EndTime = 0;
    gmEvent->eventName = eventName;
    gmEvent->eventDescription = eventDescription;
    gmEvent->status = RUNNING;
    gmEvents.Push(gmEvent);

    // keep psCharacter upto date
    client->GetActor()->GetCharacterData()->AssignGMEvent(gmEvent->id,true);

    psserver->SendSystemInfo(client->GetClientNum(),
                             "You have initiated a new event, \'%s\'.",
                             eventName.GetDataSafe());

    return true;
}

int main(){

	/* Test water tank input output
	 * output should decrease */


	double level = 0;
	double vin = 0;
	double vout = 0;
	double ref = 500;
	int period = 100;

	PIDParameters pp(1,2,1,0,0.1,period,ref,0,255,false);
	PID pid;
	pid.updateParameters(pp);
	std::cout << pp << "\n";
	for (int i = 0; i < 7000; i++){
		vin = 0.2* pid.next(level);
		pid.updateStates();
		vout = 0.1 * sqrt(2*9.82*level);
		level = level + (vin - vout) * period/1000;
		if (level < 0) level = 0;
		std::cout << ref - level << "\n";
	}
}

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

//Sets up the internal state monitor (PID for heating and Watchdog)
void internalStateSetup() {
  //TMP102.h temperature ranges from -40 to 125 C
  controller.setInputLimits(-40.0, 125.0);
  controller.setOutputLimits(0.0, 1.0); 
  controller.setBias(0.3); // Try experimenting with the bias! 
  controller.setMode(AUTO_MODE);
  controller.setSetPoint(DESIRED_INTERNAL_TEMP);  
  W.Start(WATCH_DOG_RATE); 
}

void CharCreationManager::HandleCharDelete(MsgEntry* me, Client* client)
{
    psCharDeleteMessage msg(me);
    csString charName = msg.charName;

    if(!charName.Length())
    {
        CPrintf(CON_WARNING,"Client %u sent malformed character name to deletion code!",client->GetClientNum());
        return; // No char
    }

    PID pid = psserver->CharacterLoader.FindCharacterID(client->GetAccountID(), charName);
    csString error;

    if(psserver->CharacterLoader.AccountOwner(charName, client->GetAccountID()))
    {
        // Found the char?
        if(!pid.IsValid())
        {
            psserver->SendSystemError(client->GetClientNum(),"Couldn't find character data!");
            return;
        }

        // Can we delete it?
        if(!psserver->CharacterLoader.DeleteCharacterData(pid, error))
        {
            psserver->SendSystemError(client->GetClientNum(),"Error: %s",error.GetData());
            return;
        }

        // Remove cached objects to make sure that the client gets a fresh character
        // list from the database.
        iCachedObject* obj = psserver->GetCacheManager()->RemoveFromCache(psserver->GetCacheManager()->MakeCacheName("list",client->GetAccountID().Unbox()));
        if(obj)
        {
            obj->ProcessCacheTimeout();
            obj->DeleteSelf();
        }
        obj = psserver->GetCacheManager()->RemoveFromCache(psserver->GetCacheManager()->MakeCacheName("auth", client->GetAccountID().Unbox()));
        if(obj)
        {
            obj->ProcessCacheTimeout();
            obj->DeleteSelf();
        }

        // Ok, we deleted it
        psCharDeleteMessage response(charName, me->clientnum);
        response.SendMessage();
    }
    else
    {
        CPrintf(CON_WARNING,"Character %s not deleted because of non-ownership\n", charName.GetData());
        psserver->SendSystemError(client->GetClientNum(),"You do not own that character!");
        return;
    }
}

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

/*
 * Balance control node
 */
msg_t balance_node(void *arg) {
	r2p::Node node("balance");
	r2p::Subscriber<r2p::TiltMsg, 2> tilt_sub;
	r2p::TiltMsg *tiltp;

	r2p::Publisher<r2p::PWM2Msg> pwm2_pub;
	r2p::PWM2Msg *pwmp;

	int32_t pwm = 0;

	(void) arg;
	chRegSetThreadName("balance");

	node.advertise(pwm2_pub, "pwm2");
	node.subscribe(tilt_sub, "tilt");

	PID pid;
	//pid.config(800, 0.35, 0.09, 0.02, -4000, 4000);

	pid.config(800, 0.35, 0.01, 0.02, -4000, 4000);
	//pid.config(550, 0.6, 0, 0.02, -4000, 4000);
	//pid.config(500, 990.35, 0, 0.02, -4000, 4000);
	//	pid.config(600, 0.35, 0, 0.02, -4000, 4000);
	//
	//
	//
	pid.set(angle_setpoint);

	for (;;) {
		pid.set(angle_setpoint);

		while (!tilt_sub.fetch(tiltp)) {
			r2p::Thread::sleep(r2p::Time::ms(1));
		}

		pwm = pid.update(tiltp->angle); //rad2grad
		tilt_sub.release(*tiltp);

		if (pwm2_pub.alloc(pwmp)) {
			pwmp->value[0] = (pwm - w_setpoint * 100); //FIXME
			pwmp->value[1] = -(pwm + w_setpoint * 100); //FIXME
			pwm2_pub.publish(*pwmp);
			palTogglePad(LED3_GPIO, LED3);
			palSetPad(LED4_GPIO, LED4);
		} else {
			palClearPad(LED4_GPIO, LED4);
		}

	}
	return CH_SUCCESS;
}

void loop() {

    // 22,0,0 is lowest value for sustaining oscillation
    // 1.48,0,0 is highest value for almost no reverse
    static float P = 1.48, I = 0, D = 0;

    static long setpoint = 0;

    if (Serial.available()) { // User started typing something
        if        (Serial.peek() == 'p') {
            Serial.read();
            Serial.println(F("\r\nGive a P value"));
            scanf_P(PSTR("%f"), &P);
            Serial.println(P);
        } else if (Serial.peek() == 'i') {
            Serial.read();
            Serial.println(F("\r\nGive a I value"));
            scanf_P(PSTR("%f"), &I);
            Serial.println(I);
        } else if (Serial.peek() == 'd') {
            Serial.read();
            Serial.println(F("\r\nGive a D value"));
            scanf_P(PSTR("%f"), &D);
            Serial.println(D);
        } else if (Serial.peek() == 's') {
            Serial.read();
            Serial.println(F("\r\nGive a Setpoint"));
            scanf_P(PSTR("%ld"), &setpoint);
            Serial.println(setpoint);
        } else {
            Serial.println(F("\r\nEnter new PID values"));
            scanf_P(PSTR("%f %f %f"), &P, &I, &D);
            printf_P(PSTR("%f %f %f"), P, I, D);
        }
    }
    botPID.SetPID(P, I, D);

    bot.UpdateEncoders();

    long encdists[4];
    bot.GetDists(encdists);

    long xdist, ydist;
    bot.GetPos(&xdist, &ydist);
    int wheelspd = botPID.GetOutput(xdist, setpoint, micros());
    bot.Run(0, wheelspd);
    
    //printf_P(PSTR(/*"P:%.2f I:%.2f D:%.2f "*/"curdist:%4ld setpoint:%4ld curspd:%4d\n"),
    //                      P,     I,     D,           xdist,     setpoint,  wheelspd);
    delay(10);
}

void setup(void)
{
    board.init();

    // sleep for 100ms
    board.delayMilliseconds(100);

    // flash the LEDs to indicate startup
    board.ledRedOn();
    board.ledGreenOff();
    for (uint8_t i = 0; i < 10; i++) {
        board.ledRedToggle();
        board.ledGreenToggle();
        board.delayMilliseconds(50);
    }
    board.ledRedOff();
    board.ledGreenOff();

    // initialize our RC, IMU, mixer, and PID controller
    rc.init(&board);
    imu.init(&board);
    pid.init();
    mixer.init(&board, &rc, &pid); 
    msp.init(&board, &imu, &mixer, &rc);

    // set initial time
    previousTime = board.getMicros();

    // always do gyro calibration at startup
    calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;

    // trigger accelerometer calibration requirement
    haveSmallAngle = true;

} // setup

PID EntityManager::CopyNPCFromDatabase(PID master_id, float x, float y, float z, float angle, const csString & sector, InstanceID instance, const char *firstName, const char *lastName)
{
    psCharacter * npc = NULL;
    PID new_id;

    npc = psServer::CharacterLoader.LoadCharacterData(master_id, false);
    if (npc == NULL)
    {
        return 0;
    }

    psSectorInfo* sectorInfo = CacheManager::GetSingleton().GetSectorInfoByName( sector );
    if (sectorInfo != NULL)
    {
        npc->SetLocationInWorld(instance,sectorInfo,x,y,z,angle);
    }

    if ( firstName && lastName )
    {
        npc->SetFullName( firstName, lastName );
    }

    if (psServer::CharacterLoader.NewNPCCharacterData(0, npc))
    {
        new_id = npc->GetPID();
        db->Command("UPDATE characters SET npc_master_id=%d WHERE id=%d", master_id.Unbox(), new_id.Unbox());
    }

    delete npc;

    return new_id;
}

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

int main()
{
    Network network;
    Accelerometer imu;
    imu.bypassDrift();
    motors.setToZero();

    signal(SIGABRT, sigHandler);
    signal(SIGINT, sigHandler);
    signal(SIGKILL, sigHandler);
    signal(SIGQUIT, sigHandler);
    signal(SIGTERM, sigHandler);

    float ypr[3];
    while(true) {
        if(imu.getFIFOCount() > 42) {
            imu.getYawPitchRoll(ypr);

            float p_computed = p_pid.compute(ypr[1], p_target), r_computed = r_pid.compute(ypr[2], r_target);
            motors.setSpeed(MOTOR_FL, throttle + r_computed - p_computed);
            motors.setSpeed(MOTOR_BL, throttle + r_computed + p_computed);
            motors.setSpeed(MOTOR_FR, throttle - r_computed - p_computed);
            motors.setSpeed(MOTOR_BR, throttle - r_computed + p_computed);

            network.send(SET_MEASURED_VALUES, ypr, sizeof(float)*3, false);
        }
    } 

    exit(EXIT_SUCCESS);
}