Exemple #1
0
void PID_path_arc(int32_t x,int32_t y,int32_t sx, int32_t sy , int32_t sa)
{
	PID_target_X = x;
	PID_target_Y = y;
	PID_start_X = sx;
	PID_start_Y = sy;
	PID_start_angle = sa;
	
	int32_t a = (((x-sx)*int_cos(sa)) - (y-sy)*int_sin(sa))/10000;
	int32_t b = (((x-sx)*int_sin(sa)) + (y-sy)*int_cos(sa))/10000;
	if(b < 0) b = -b; 
	
	if(a >= 0)
		PID_dir = 1; // right
	else
	{
		PID_dir = -1;
		a = -a;
	}

	PID_radius = (b * b / a + a)>>1;
	//int16_t angle = int_arccos((r - a) / r);
	
	PID_center_X = sx + PID_dir * PID_radius * int_cos(sa)/10000;
	PID_center_Y = sy - PID_dir * PID_radius * int_sin(sa)/10000;
	
	PID_status = 7;	
}
Exemple #2
0
static u32 int_goertzel(s16 x[], u32 N, u32 freq)
{
	/* We use the Goertzel algorithm to determine the power of the
	 * given frequency in the signal */
	s32 s_prev = 0;
	s32 s_prev2 = 0;
	s32 coeff = 2*int_cos(freq);
	u32 i;

	u64 tmp;
	u32 divisor;

	for (i = 0; i < N; i++) {
		s32 s = x[i] + ((s64)coeff*s_prev/32768) - s_prev2;
		s_prev2 = s_prev;
		s_prev = s;
	}

	tmp = (s64)s_prev2 * s_prev2 + (s64)s_prev * s_prev -
		      (s64)coeff * s_prev2 * s_prev / 32768;

	/* XXX: N must be low enough so that N*N fits in s32.
	 * Else we need two divisions. */
	divisor = N * N;
	do_div(tmp, divisor);

	return (u32) tmp;
}
Exemple #3
0
static s32 int_cos(u32 x)
{
	u32 t2, t4, t6, t8;
	s32 ret;
	u16 period = x / INT_PI;
	if (period % 2)
		return -int_cos(x - INT_PI);
	x = x % INT_PI;
	if (x > INT_PI/2)
		return -int_cos(INT_PI/2 - (x % (INT_PI/2)));
	/* Now x is between 0 and INT_PI/2.
	 * To calculate cos(x) we use it's Taylor polinom. */
	t2 = x*x/32768/2;
	t4 = t2*x/32768*x/32768/3/4;
	t6 = t4*x/32768*x/32768/5/6;
	t8 = t6*x/32768*x/32768/7/8;
	ret = 32768-t2+t4-t6+t8;
	return ret;
}
Exemple #4
0
void PID_set_target_distance(int32_t pos)
{
	PID_disable();
	
	PID_prev_error =0;
	PID_error =0;
	PID_integration = 0;
	
	PID_u = 0;
	
	PID_desire_angle = position_get_angle();//modified
	PID_start_X = position_get_X();
	PID_start_Y = position_get_Y();
	
	PID_cos = int_cos(PID_desire_angle);
	PID_sin = int_sin(PID_desire_angle);
	PID_target_X = PID_start_X + pos*int_sin(PID_desire_angle)/10000;
	PID_target_Y = PID_start_Y + pos*int_cos(PID_desire_angle)/10000;
	
	if(pos < 0)
	PID_desire_angle = (PID_desire_angle+180)%360;
	
	PID_set_status(3);
}
Exemple #5
0
void PID_set_target_angle(int16_t angle)
{
	PID_disable();
	
	PID_prev_error =0;
	PID_error =0;
	PID_integration = 0;
	
	PID_u = 0;
	
	PID_desire_angle = angle;
	PID_start_X = position_get_X();
	PID_start_Y = position_get_Y();
	
	PID_cos = int_cos(PID_desire_angle);
	PID_sin = int_sin(PID_desire_angle);
	
	PID_set_status(1);
}
Exemple #6
0
void PID_set_target_XYAngle_back(int32_t X,int32_t Y,int32_t angle)
{
	PID_disable();
	
	PID_prev_error =0;
	PID_error =0;
	PID_integration = 0;
	
	PID_u = 0;
	
	PID_start_X = position_get_X();
	PID_start_Y = position_get_Y();
	PID_target_X = X;
	PID_target_Y = Y;
	PID_desire_angle = angle;
	PID_cos = int_cos((angle + 180)%360);
	PID_sin = int_sin((angle + 180)%360);
	
	PID_set_status(3);
}
Exemple #7
0
int PID_arc_done(void)
{     
	if(((position_get_X() - PID_target_X)*int_sin(PID_desire_angle) + (position_get_Y() - PID_target_Y)*int_cos(PID_desire_angle)>0)
		&& ((position_get_X() - PID_target_X)*(position_get_X() - PID_target_X)+(position_get_Y() - PID_target_Y)*(position_get_Y() - PID_target_Y)<=50000))
		return 1;
	else
		return 0;
}
Exemple #8
0
void PID_calculation_u(void)
{	
	static int32_t PID_current_angle;    //chu zao yin
	static int32_t PID_current_X;
	static int32_t PID_current_Y;
	static int16_t PID_angle;
	static int32_t d = 0;
	static int16_t diff_x=0, diff_y=0;
	static int16_t sita=0, arfa=0;
	static int8_t qua = 0;
	
	if(PID_status)
	{
		PID_current_angle = position_get_angle();//modified
		PID_current_angle = (PID_current_angle>180)?(PID_current_angle-360):PID_current_angle;
		PID_current_X = position_get_X();
		PID_current_Y = position_get_Y();
			
		if(PID_status&_BV(0))
		{
			if (PID_status&_BV(2)) {//arc mode
				diff_x = PID_current_X - PID_center_X;
				diff_y = PID_current_Y - PID_center_Y;			
				if (PID_dir==1) {
					if(diff_x<0 && diff_y >= 0){ //0-90
						sita = int_arc_tan_ver2(diff_y,-diff_x);
					}
					else if(diff_x>=0 && diff_y>=0){//90-180
						sita = int_arc_tan_ver2(diff_x,diff_y) + 90;
					}
					else if(diff_x>=0 && diff_y<0){//-180 - -90
						sita = int_arc_tan_ver2(diff_x,diff_y) - 90;
					}
					else if(diff_x<0 && diff_y<0){ //-90 - 0
						sita = int_arc_tan_ver2(-diff_y,diff_x);
					}
				}
				else if (PID_dir==-1) {
					if(diff_x>0 && diff_y>=0){ //0-90
						sita = int_arc_tan_ver2(diff_y,diff_x);
					}
					else if(diff_x<=0 && diff_y>=0){//90-180
						sita = int_arc_tan_ver2(-diff_x,diff_y) + 90;
					}
					else if(diff_x<=0 && diff_y<0){//-180 - -90
						sita = int_arc_tan_ver2(-diff_x,diff_y) - 90;
					}
					else if(diff_x>0 && diff_y<0){ //-90 - 0
						sita = int_arc_tan_ver2(diff_y,diff_x);
					}		
				}
				
				qua = sita/45;
				switch(qua){
					case 0:
					case 3:
					case -3:
					case 4:
					case -4:
						d = PID_radius + (int32_t)diff_x*10000/int_cos(sita)*(int32_t)PID_dir;
						break;
					case 1:
					case 2:
					case -1:
					case -2:
						d = PID_radius - (int32_t)diff_y*10000/int_sin(sita);
						break;
				}
				
				PID_desire_angle=PID_start_angle+sita*(int32_t)PID_dir;
				
				arfa = int_arc_sin(10000*d/RAD);
				
				PID_error = arfa*(int32_t)PID_dir + PID_current_angle - sita*(int32_t)PID_dir;					
			}
			else {//line mode
				PID_error_position = ((PID_current_X - PID_start_X)*PID_cos - (PID_current_Y - PID_start_Y)*PID_sin)/10000;
				PID_angle = int_arc_sin(10000*PID_error_position/RAD);
				
				//To fix the angle difference problem near negative y-axis(jumping from 17x to -17x)
				int16_t tmp=PID_current_angle - PID_desire_angle;
				if (tmp>=180)
					tmp -= 360;
				else if (tmp<=-180)
					tmp += 360;
					
				PID_error= tmp + PID_angle;			
			}

			PID_integration = KI * PID_error + PID_integration;

			//Integration error limit
			if(PID_integration >= IL)
				PID_integration = IL;
			
			PID_u =KP * PID_prev_error + PID_integration +KD * (PID_error - PID_prev_error);			
			//Error limit
			if(PID_u > UL)
			PID_u = UL;
			else if(PID_u < - UL)
			PID_u = -UL;
			
			PID_prev_error = PID_error;
		}
		
		lcd_clear();
		lcd_printxys(0,0,"X:%d",PID_current_X);
		lcd_printxys(10,0,"Y:%d",PID_current_Y);
		lcd_printxys(0,1,"A:%d",arfa);
		lcd_printxys(0,2,"u: %ld",PID_u);
		lcd_printxys(10,2,"pe: %d",PID_error);
	}
}