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