C++ PIDController类代码示例

	Robot() :
            robotDrive(frontLeftChannel, rearLeftChannel,
                       frontRightChannel, rearRightChannel), // initialize variables in same
            stick(joystickChannel)                           // order as declared above.
    {
	    rotateToAngleRate = 0.0f;
        robotDrive.SetExpiration(0.1);
        robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);	// invert left side motors
        robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);	// change to match your robot
        try {
            /* Communicate w/navX MXP via the MXP SPI Bus.                                       */
            /* Alternatively:  I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
            /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details.   */
            ahrs = new AHRS(SPI::Port::kMXP);
        } catch (std::exception ex ) {
            std::string err_string = "Error instantiating navX MXP:  ";
            err_string += ex.what();
            DriverStation::ReportError(err_string.c_str());
        }
        turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
        turnController->SetInputRange(-180.0f,  180.0f);
        turnController->SetOutputRange(-1.0, 1.0);
        turnController->SetAbsoluteTolerance(kToleranceDegrees);
        turnController->SetContinuous(true);

        /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
        /* tuning of the Turn Controller's P, I and D coefficients.            */
        /* Typically, only the P value needs to be modified.                   */
        LiveWindow::GetInstance()->AddActuator("DriveSystem", "RotateController", turnController);
        if ( ahrs ) {
            LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
        }
    }

int main(int argc, char *argv[])
{
	PIDController* pid = new PIDController(K_P,K_I,K_D);

    pid->on(); // Turn PID controller on
    pid->targetSetpoint(SETPOINT); // Change desired setpoint to SETPOINT
    
    double t = 0; // Init with time t=0
    double T = 0.01; // Period

    double controlVariable = 0; // Init with zero actuation
    double processVariable = 0; // Init with zero position
   
    cout << "Simulation running..." << endl;
    
    ofstream writeToCsv;
    writeToCsv.open("PIDexample.csv");
    writeToCsv << "Time,Setpoint,Output" << endl;

    while(t < 20) {
        writeToCsv << t << "," << pid->getSetpoint() << "," << processVariable << endl;
        
        controlVariable = pid->calc(processVariable); // Calculate next controlVariable
        processVariable = LaplaceInversion(xferFn, t, 1e-8); // Simulate plant with TF 1/((s+a)(s+b))

        usleep(T*pow(10,6)); // 100ms delay to simulate actuation time
        t += T; // Increment time variable by 100ms
    }

	writeToCsv.close();
    cout << "Simulation complete! Output saved to PIDexample.csv." << endl;
	
    return 0;
}

int main(int argc, char** argv)
{
    Motor* motor = new Motor(0);
    Encoder* encoder = new Encoder(motor);
    PIDController* pidController = new PIDController(motor, encoder, POSITION_REV, 1, 0, 0);
    pidController->setSetpoint(10);
    pidController->enable();
    
    Motor* motor2 = new Motor(1);
    Encoder* encoder2 = new Encoder(motor2);
    PIDController* pidController2 = new PIDController(motor2, encoder2, SPEED, 0, 0, 0, .4/20);
    pidController2->setSetpoint(20);
    pidController2->enable();
    
    int ticks = 0;
    
    while(!pidController->onTarget())
    {
        pidController->update();
        pidController2->update();
        std::cout << "tick:\t" << ++ticks << "\tcurrent position:\t" << encoder->getPosition() << "\tcurrent speed:\t" << encoder->getSpeed() << "\n";
        std::cout << "tick:\t" << ticks << "\tcurrent speed:\t" << encoder2->getSpeed() << "\n";
    }
    
    std::cout << "done with position " << encoder->getPosition() << "\n";
    
    return 0;
}

static void pidInterrupt(void* object) {
    PIDController* pid = object;

    if(pid->enabled) {
        // process the PID data //
        pid->data.input = pid->pidInput(pid->state);
        PID_calculate(&pid->data);
        pid->pidOutput(pid->state, pid->data.output);
    }
}

void ShooterArm::SetTargetAngle(float tgtAngle)
{
    _targetAngle = tgtAngle;
    _onStage2 = false;
    _onStage3 = false;
    PIDController *pid = GetPIDController();
    pid->SetPID(_bothStage_P,_stage_1_I,_stage_1_D);
    GetPIDController()->SetAbsoluteTolerance(fabs(_stage_1_tolerance * VOLTAGE_DEG_SCALAR));
    _voltageTarget = DEGREES_TO_VOLTAGE(tgtAngle);
    GetPIDController()->SetSetpoint(_voltageTarget);
    Enable();
}

	void TeleopPeriodic() {
		if(m_driver->GetRawButton(BUTTON_LB)) {
			// PYRAMID
			m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawButton(BUTTON_RB)) {
			// FEEDER
			m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
			m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
			m_PIDController->Enable();
		} else {
			// MANUAL CONTROL
			m_PIDController->Disable();
			
			m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
			m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
		}
		
		// ----- PRINT -----
		SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
		SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
		
	} // TeleopPeriodic()

	/**
	 *  Uses a PIDController and an array of setpoints to switch and maintain elevator positions.
     *  The elevator setpoint is selected by a joystick button.
	 */
	void OperatorControl() {
		pidController->SetInputRange(0, 5); //0 to 5V
		pidController->SetSetpoint(setPoints[0]); //set to first setpoint

		int index = 0;
		bool currentValue;
		bool previousValue = false;

		while (IsOperatorControl() && IsEnabled()) {
			pidController->Enable(); //begin PID control

			//when the button is pressed once, the selected elevator setpoint is incremented
			currentValue = joystick->GetRawButton(buttonNumber);
			if (currentValue && !previousValue) {
				pidController->SetSetpoint(setPoints[index]);
				index = (index + 1) % (sizeof(setPoints)/8); //index of elevator setpoint wraps around
			}
			previousValue = currentValue;
		}
	}

	void RobotInit() override {
		lw = LiveWindow::GetInstance();
		drive->SetExpiration(20000);
		drive->SetSafetyEnabled(false);
		//Gyroscope stuff
		try {
			/* Communicate w/navX-MXP via the MXP SPI Bus.                                       */
			/* Alternatively:  I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
			/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details.   */
			ahrs = new AHRS(SPI::Port::kMXP);
		} catch (std::exception ex) {
			std::string err_string = "Error instantiating navX-MXP:  ";
			err_string += ex.what();
			//DriverStation::ReportError(err_string.c_str());
		}

		if (ahrs) {
			LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
			ahrs->ZeroYaw();

			// Kp	  Ki	 Kd		Kf    PIDSource PIDoutput
			turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
					ahrs, this);
			turnController->SetInputRange(-180.0f, 180.0f);
			turnController->SetOutputRange(-1.0, 1.0);
			turnController->SetAbsoluteTolerance(2); //tolerance in degrees
			turnController->SetContinuous(true);
		}
		chooser.AddDefault("No Auto", new int(0));
		chooser.AddObject("GyroTest Auto", new int(1));
		chooser.AddObject("Spy Auto", new int(2));
		chooser.AddObject("Low Bar Auto", new int(3));
		chooser.AddObject("Straight Spy Auto", new int(4));
		chooser.AddObject("Adjustable Straight Auto", new int(5));
		SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
		SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
		SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
		SmartDashboard::PutData("Auto Modes", &chooser);
		liftdown->Set(false);
		comp->Start();
	}

	Robot()
	{
#if BUILD_VERSION == COMPETITION
		liftMotor = new CANTalon(CHAN_LIFT_MOTOR);
		liftMotor2 = new CANTalon(CHAN_LIFT_MOTOR2);
#else
		liftMotor = new CANJaguar(CHAN_LIFT_MOTOR);
		liftMotor2 = new CANJaguar(CHAN_LIFT_MOTOR2);
#endif
		liftEncoder = new Encoder(CHAN_LIFT_ENCODER_A, CHAN_LIFT_ENCODER_B, false, Encoder::EncodingType::k4X);
		liftEncoder->SetDistancePerPulse(LIFT_ENCODER_DIST_PER_PULSE);
		liftEncoder->SetPIDSourceParameter(liftEncoder->kDistance);
#if BUILD_VERSION == COMPETITION
		liftEncoder->SetReverseDirection(true);
#endif
		controlLift = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor);
		controlLift->SetContinuous(true); //treat input to controller as continuous; true by default
		controlLift->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
		controlLift->Disable(); //do not enable until in holding position mode
		controlLift2 = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor2);
		controlLift2->SetContinuous(true); //treat input to controller as continuous; true by default
		controlLift2->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
		controlLift2->Disable(); //do not enable until in holding position mode
		liftLimitSwitchMin = new DigitalInput(CHAN_LIFT_LOW_LS);
		liftLimitSwitchMax = new DigitalInput(CHAN_LIFT_HIGH_LS);
		joystick = new Joystick(CHAN_JS);
	}

int main(int argc, char** argv) {
	ros::init(argc,argv,"mk_behaviors");
	ros::NodeHandle nh;
	ros::Subscriber vispose_sub = nh.subscribe<mk_msgs::ARMarker>("/mikrokopter/vision/ar_pose_marker", 1000, &getVisualPos);
	ros::Subscriber sensororient = nh.subscribe<mk_msgs::sensorData>("/mikrokopter/sensor/data",1000,&getSensorOrient);
	ros::Subscriber ugvposition = nh.subscribe<geometry_msgs::Point>("/mikrokopter/desired",1000,&getUGVPose);
	ros::Publisher motorcomout = nh.advertise<mk_msgs::motorCommands>("/mikrokopter/commands/motor", 5);


	double desiredZ = atof(argv[3]);

	vispose.position.x = 0;
	vispose.position.y = 0;
	vispose.position.z = 0;
	vispose.orientation.x = 1;
	vispose.orientation.y = 0;
	vispose.orientation.z = 0;
	vispose.orientation.w = 0;

	PIDController pidcontroller;
	pidcontroller.setDesiredRPY(-1.57);
	pidcontroller.setPIDParameters(atof(argv[4]),atof(argv[5]),atof(argv[6]));
	pidcontroller.setForces(2.2, atof(argv[7]), atof(argv[8]));

	while(ros::ok()){
		pidcontroller.setCurrentPosition(vispose.position.x,
											vispose.position.y,
											vispose.position.z,
											roll,
											pitch,
											tf::getYaw(vispose.orientation));
		pidcontroller.setTime(ros::Time::now().toNSec());

		double DesiredPosModRadius = 4;
		double DesiredPosModFreq = 0.1;
		double DesiredPosModStartTime = 0;
		desiredX += 0.1; -DesiredPosModRadius*cos(2*M_PI*DesiredPosModFreq*(DesiredPosModStartTime - ros::Time::now().toSec())) + DesiredPosModRadius;
		desiredY = 1;-DesiredPosModRadius*sin(2*M_PI*DesiredPosModFreq*(DesiredPosModStartTime - ros::Time::now().toSec()));

		desiredZ = - 10;
		pidcontroller.setDesiredPosition(desiredX, desiredY, desiredZ);
		motorcomout.publish(pidcontroller.pid());
		ros::Duration(0.05).sleep();
		ros::spinOnce();
	}

	return 0;
}

/* ....................................................................... */
void
SliderServoMotor::operator()(ODEObject* object)
{

    SliderJoint*     slider = object->asSliderJoint() ;
    PS_ASSERT1( slider != NULL ) ;

    World*      world = object->getWorld() ;
    PS_ASSERT1( world != NULL ) ;


    PIDController*  pid = getPIDController() ;

    ooReal  vel = 0.0 ;

    if( pid ) {
        vel = pid->solve( m_position - slider->getPosition(), world->getCurrentStepSize() ) ;
    } else {
        vel = m_position - slider->getPosition() ;
    }



    if( m_max_vel >= 0.0 ) {

        vel = osg::clampTo( vel, -m_max_vel, m_max_vel ) ;
    }





    slider->setParam( dParamFMax, m_force ) ;
    slider->setParam( dParamVel, vel ) ;


    traverse(object) ;
}

	void UpdateDashboard() {
		float r = 0.00001 * i;
		SmartDashboard::PutNumber("State", currentState + r);
		SmartDashboard::PutNumber("PID Turn Error",
				turnController->GetError() + r);
		SmartDashboard::PutNumber("PID Target",
				turnController->GetSetpoint() + r);
//		SmartDashboard::PutBoolean("Straight", straight);
		SmartDashboard::PutData("test", turnController);
		SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r);
		SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r);
		SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r);
		SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r);
		SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r);
		SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r);
		SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r);
		SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r);
		SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r);
		SmartDashboard::PutNumber("Constant Lift", constantLift);
		SmartDashboard::PutNumber("Rotate Rate", rotateRate + r);
		i = (i + 1) % 2;
		printf("2.1");
//		.PutLong("test1.2", 1337);
		printf("2.2");
//		mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime());
		printf("2.3");
//		mqServer.PutString("test1.1","YOLO_SWAGINS");
		printf("2.4");
//		SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1"));
//		SmartDashboard::PutNumber("test1", mqServer.GetDouble("test"));
//		SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2"));
//		SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime());
	}

	void AutonomousGyroTurn() {
		switch (currentState) {
		case 1:

			timer->Reset();
			timer->Start();
			//State: Stopped
			//Transition: Driving State
			currentState = 2;
			break;
		case 2:
			//State: Driving
			//Stay in State until 2 seconds have passed--`
			//Transition: Gyroturn State
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				currentState = 3;
				turnController->SetSetpoint(90);
				turnController->Enable();
			}
			break;
		case 3:
			//State: Gyroturn
			//Stay in state until navx yaw has reached 90 degrees
			//Transition: Driving State
			drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
//			if (ahrs->GetYaw() >= 90) {
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			//State:Driving
			//Stay in state until 2 seconds have passed
			//Transition: State Stopped
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) {
				currentState = 5;
				timer->Stop();
			}
			break;
		case 5:
			//State: Stopped
			drive->TankDrive(0.0, 0.0);
			break;

		}

	}

	void Disabled()
	{
		printf("AAAAAAAAAAAAAAAAAAAAAAAA");
		decel = false;
		done = false;
		theLift->Reset();
	}

	float DistToSetpoint()
	{
		if(!(controlLift->IsEnabled()))
				return UNINIT_VAL;
		else
			return (liftEncoder->GetDistance() - pidPosSetPoint);
	}