C++ (Cpp) PIDControl Examples

Example #1

File: controller.cpp Project: stevendaniluk/ghost

/* poseCallback
*
* Callback for car pose. Stores the vehicles state, then calls PIDControl
* to issue the necessary controls.
*/
void Controller::poseCallback(const nav_msgs::Odometry::ConstPtr& msg) {
	// Save new state
	x_ = msg->pose.pose.position.x;
	y_ = msg->pose.pose.position.y;
	v_ = sqrt(pow(msg->twist.twist.linear.x, 2) + pow(msg->twist.twist.linear.y, 2));
	dpsi_ = msg->twist.twist.angular.z;
	
	// Convert from geometry_msgs to tf quaternion, then to roll-pitch-yaw
	tf::Quaternion quat;
	tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);
	double dud_roll, dud_pitch;
	tf::Matrix3x3(quat).getRPY(dud_roll, dud_pitch, psi_);
	
	t_prev_ = t_now_;
	t_now_ = msg->header.stamp;
		
	// Search for the closest position on the racing line
	if(first_pose_) {
		globalIndexSearch();
		first_pose_ = false;
		
		// Need a previous message to continue and issue controls
		return;
	} else {
		localIndexSearch();
	}
	
	// Drive
	PIDControl();
}

Example #2

File: milestone2_MotorControl(PID)3.c Project: Yamoahs/ENCE361_Helicopter_project

int main(void)
{
	unsigned int i;
	int sum = 0;
	int current = 0;
	int hgt_percent = 0;
	int degrees = 0;

	STATE = IDLE;


	initClock();
	initADC();
	initYaw();
	initMotorPin();
	initDisplay();
	intButton();
	initConsole();
	initPWMchan();
	initCircBuf (&g_inBuffer, BUF_SIZE);


	// Enable interrupts to the processor.
	IntMasterEnable();

	while (1)
	{
		//double dt = SysCtlClockGet() / SYSTICK_RATE_HZ;
		degrees = yawToDeg();


		// Background task: calculate the (approximate) mean of the values in the
		// circular buffer and display it.
		sum = 0;
		for (i = 0; i < BUF_SIZE; i++) {
			current = readCircBuf (&g_inBuffer);
			sum = sum + current;

		}
		int newHght = ADC_TO_MILLIS(sum/BUF_SIZE);
		if(initialRead != 0)
		{
			hgt_percent = calcHeight(initialRead, newHght);

		}
		if (STATE == FLYING || STATE == LANDING){
			PIDControl(hgt_percent, SysCtlClockGet() / SYSTICK_RATE_HZ);
			PWMPulseWidthSet (PWM_BASE, PWM_OUT_1, period * main_duty / 100);
			PWMPulseWidthSet (PWM_BASE, PWM_OUT_4, period * tail_duty / 100);
		}

		displayInfo((int)initialRead, hgt_percent, degrees);
	}
}

Example #3

File: IP.cpp Project: stmharry/ControlExperimentFinal

int main(void) {
	/********/
	/* INIT */
	/********/
	Pause(10000);
	SetUart0(115200, 2);
	InitialGpioState(PD8, 0, 2);
	InitialGpioState(PD9, 0, 2);
	
	myLCD.BacklightOn(0);
	myLCD.SetBacklight(255);
	
	SetTm2Decoder(1, 0, 65535); // IP
	SetTm3Decoder(3, 0, 65535); // WHEEL
	SetTm4Counter(0, 0, 7199, 10000);
	
	unsigned short T = 0,    PrevT = 0, Multiplier = 0;
	float          Time = 0, PrevTime = 0;
	short IPEncoder, MotorEncoder, ControllerState = 1;
	
	while(true) {
		Transmit("0");
		
		/********/
		/* TIME */
		/********/
		PrevT = T;
		GetTm4CounterValue(T);
		if(T < PrevT)
			Multiplier += 1;	
		PrevTime = Time;
		Time = Multiplier + float(T) / 10000;
		
		/*********/
		/* BLOCK */
		/*********/
		GetTm2DecoderValue(IPEncoder);
		GetTm3DecoderValue(MotorEncoder);	
		if(ControllerState == 0)
			SwingControl(IPEncoder, Time, ControllerState);
		else
			PIDControl(IPEncoder, MotorEncoder, Time - PrevTime);
		
		/********/
		/* SEND */
		/********/
		Transmit(int(Time*10000), false);
		Transmit(int(IPEncoder), false);
		Transmit(int(MotorEncoder), true);
		
		/*********/
		/* CATCH */
		/*********/
		short IPEncoderError = IPEncoder - ENCODER_RANGE / 2;
		if( (ControllerState == 1) && ((IPEncoder > 200) || (IPEncoder < -200)) )
			break;
		Pause(50);
	}
	StopMotor();	
	return 0;
}