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