Demo entry 6783509

Código do controlador PID

   

Submitted by anonymous on Feb 15, 2019 at 00:28
Language: C. Code size: 1.2 kB.

def_dir = get_angle();
if(def_dir < 0){
def_dir = 360 + def_dir;
}

while(1){
	angle=get_angle();
	angle_r = angle;
	// corrigir variacao de 180 a -180 graus
	if(angle_r < 0){
		angle_r = 360 + angle_r;
	}
	dir_dif = angle_r - def_dir;
	
	gps_monitor(&gps_azimuth, &gps_haversine, &actual_pos);
	
	// Caso a distancia ate o marco seja menor que 10 metros, 
	// entao usa a camera para alterar o referencial de direcao
	if(gps_haversine < 10){
		def_dir = track_mark();
		dir_dif = angle_r - def_dir;
	}else{
		def_dir = gps_azimuth;
		dir_dif = angle_r - def_dir;
	}	
	if(dir_dif < -180){
		dir_dif = dir_dif +360;
	}else if(dir_dif > 180){
		dir_dif = (360 - dir_dif)*(-1);
	}
	
	err = dir_dif;
	
	KP = Kp*err;
	KI = Ki*err_1;
	KD = Kd*err_2;
	
	control = 1.8032*control_1 - 0.8032*control_2 + KP*3.18 - KI*5.5391 + KD*2.396;
	
	err_2 = err_1;
	err_1 = err;
	
	if(control > 10){
		control = 10;
	}
	if(control < -10){
		control = -10;
	}
	control_2 = control_1;
	control_1 = control;
	
	pwm_dir = control + 50;
	if(pwm_dir <40 ){
		pwm_dir =40;
	}else if(pwm_dir >60){
		pwm_dir = 60;
	}
	delay(10);
	send_spi_esp32(pwm_dir);
}

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).