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

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

/*Each time this is called, this pets the watchdog and computes how much the heater must be powered in order to maintain 
the desired temperature. If you every want to change the internal workings of the system, this is the function where you put that 
code*/
void internalStateLoop(const void *context) {
    //Pet the watchdog
    W.Pet();
    controller.setProcessValue(internal_temp); //We won't actually read from the TMP 102.h, we'll use the most recent internal temp variable (global).
    // Set the new output. 
    heater = controller.compute();
    printf("What should the output be? %f\n", controller.compute());
    // Now check for termination conditions
    // 1. If the GPS lat,lon exceed the permitted bounds, cut down.
    // 2. If you receive an iridum command telling you to end the flight, cut down.
    // 3. If you've not received an Iridium command in a while (5 hrs), cut down. 
}

void update_speed_and_heading()
{
	if(distance_to_current_nav(degToRad((double)NMEA::getLatitude()), degToRad((double)NMEA::getLongitude())) < WAYPOINT_RADIUS)
		go_next_nav();

	nav_list_t * current_nav = get_current_nav();

	bearing = compass.getHeadingXYDeg();
	heading = startHeading(degToRad(NMEA::getLatitude()), degToRad(NMEA::getLongitude()), current_nav->latitude, current_nav->longitude)*(180.0/M_PI);
	headingPid.setProcessValue(heading_delta(heading,bearing));
	speedOverGroundPid.setProcessValue(NMEA::getSpeed());
	#ifdef SPEED_PID_CALIBRATION
		bearingCompensation = 0;
	#else
		bearingCompensation = headingPid.compute();
	#endif
	#ifdef BEARING_PID_CALIBRATION
		speedOverGroundCompensation = 0;
	#else
		speedOverGroundCompensation = speedOverGroundPid.compute();
	#endif
	leftThrottle = ((speedOverGroundCompensation - bearingCompensation) < THROTTLE_LIMIT) ? (speedOverGroundCompensation - bearingCompensation) : THROTTLE_LIMIT;
	rightThrottle = ((speedOverGroundCompensation + bearingCompensation) < THROTTLE_LIMIT) ? (speedOverGroundCompensation + bearingCompensation) : THROTTLE_LIMIT;
}

void RosAriaNode::Mas1ToSla_cb( const geometry_msgs::PointStampedConstPtr &msg)
{
	// Master 1 Position
	Vm1 = msg->point.x;
	Xm1 = Xm1 + Vm1;
	// Master force
	Fk1 = msg->point.y;
	// Master 1 Positive Energy
	mst1_slv_cmd_P = msg->point.z;


	Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position

	Xsprv = Xs;
	Position = robot->getPose();
	Xs = Position.getX();
	delta = Xs - Xsprv;

	Vs = PosController.compute(Xsd,Xs);

	// Fs - Sum
	Fs = K_force*(Xsd - Xs);
	Fs1 = alpha*Fs;
	Fs2 = (1-alpha)*Fs;
	/*
	 * Master 1 - Slave Channel
	 */

	// Calculate Negative Energy and dissipate Active energy
	if (Vm1*Fs1>0)
	{
		mst1_slv_cmd_N -=Vm1*Fs1;
	}
	else
	{
		//Do nothing
	}

	// PC:
	if (mst1_slv_cmd_N+mst1_slv_cmd_P<0)
	{
		mst1_slv_cmd_N +=Vm1*Fs1;    // backward 1 step
		Xm1 = Xm1 - Vm1;			// backward 1 step
		// Modify Vm1
		if (Fs1*Fs1>0)
			Vm1 = (mst1_slv_cmd_N+mst1_slv_cmd_P)/Fs1;
		else
			Vm1 = 0;
		//Update
		Xm1 = Xm1 + Vm1;
		Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
		Vs = PosController.compute(Xsd,Xs);
		// Modify Fs ????

		mst1_slv_cmd_N -=Vm1*Fs1;
	}

	/*
	 * Slave - Master 1 Channel
	 */
	// Calculate Positive Energy

	if (Fk1*Vs>0)
	{
		//sla_mst1_cmd_P += Fk1*Vs;
		sla_mst1_cmd_P += Fk1*delta;
	}
	else
	{
		//Do nothing
	}

	/*
	 * Master 2 - Slave Channel
	 */

	// Calculate Negative Energy and dissipate Active energy
	if (Vm2*Fs2>0)
	{
		mst2_slv_cmd_N -=Vm2*Fs2;
	}
	else
	{
		//Do nothing
	}

	// PC:
	if (mst2_slv_cmd_N+mst2_slv_cmd_P<0)
	{
		mst2_slv_cmd_N +=Vm2*Fs2;    // backward 1 step
		Xm2 = Xm2 - Vm2;			// backward 1 step
		// Modify Vm1
		if (Fs2*Fs2>0)
			Vm2 = (mst2_slv_cmd_N+mst2_slv_cmd_P)/Fs2;
		else
			Vm2 = 0;
		//Update
		Xm2 = Xm2 + Vm2;
		Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
		Vs = PosController.compute(Xsd,Xs);
//.........这里部分代码省略.........

void MotorControl::update_omega(void) {
    pid->setProcessValue(_compass->measure_angle(target_angle_));
    omega = pid->compute();
    move(power_, move_angle_);
}