int32_t trajectory_get_nearest_angle(trajectory_manager_t *t,int32_t angle)
{
int32_t ca = angle;
int32_t cb = angle + 2L*ROBOT_IMP_PI;
int32_t cc = angle + 2L*ROBOT_IMP_PI;
if(abs(ca - position_get_angle(t->pm)) > abs(cb -position_get_angle(t->pm)))
{
ca = cb;
}
if(abs(ca - position_get_angle(t->pm)) > abs(cc -position_get_angle(t->pm)))
{
ca = cc;
}
return ca;
}
int PID_angle_done(void)
{
if(abs(position_get_angle() - PID_desire_angle)<5)
return 1;
else
return 0;
}
void PID_set_target(void)
{
PID_disable();
PID_prev_error =0;
PID_error=0;
PID_integration = 0;
PID_u = 0;
PID_desire_angle = position_get_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_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_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);
}
}
void trajectory_tick(void *arg)
{
trajectory_manager_t * t = (trajectory_manager_t *)arg;
// printf("debugdispo %ld %ld\n",t->last,t->current);
if(t->current != t->last)
{
trajectory_dest_t *p = t->points + t->current;
if((!p->flags & ALREADY_INIT))
{
p->startingD = position_get_distance(t->pm);
p->startingA = position_get_angle(t->pm);
p->flags |= ALREADY_INIT;
if(p->type == XYPION || p->type == XYPIONBACK){
double acible = atan2(p->xy.y-position_get_y(t->pm),p->xy.x-position_get_x(t->pm));
p->xy.x-=position_cm2imp(t->pm,fxx_from_double(14))*cos(acible);
p->xy.y-=position_cm2imp(t->pm,fxx_from_double(14))*sin(acible);
}
}
if(p->type == D)
{
int32_t cons = p->d.d + p->startingD;
if(abs(cons - position_get_distance(t->pm)) < p->d.prec_d)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,p->startingA);
asserv_set_distance(t->ass,cons);
}
}
else if(p->type == D_NOA)
{
int32_t cons = p->d.d + p->startingD;
if(abs(cons - position_get_distance(t->pm)) < p->d.prec_d)
{
trajectory_next(t);
}
else
{
//asserv_set_no_angle(t->ass);
asserv_set_distance(t->ass,cons);
}
}
else if(p->type == AREL)
{
int32_t cons = p->a.a + p->startingA;
if(abs(cons - position_get_angle(t->pm)) < p->a.prec_a)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,cons);
}
}
else if(p->type == A)
{
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t cons = trajectory_get_nearest_angle(t,cur + p->a.a);
if(abs(cons - position_get_angle(t->pm)) < p->a.prec_a)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,cons);
}
}
else if(p->type == XYBACK || p->type == XYPIONBACK)
{
int32_t tarx = p->xy.x - position_get_x(t->pm);
int32_t tary = p->xy.y - position_get_y(t->pm);
if(tarx * tarx + tary * tary < (p->xya.prec_xy * p->xya.prec_xy)) //yavait pas le carré sur la précision avant !
{
trajectory_next(t);
}
else
{
int32_t acible = position_rad2imp(t->pm,fxx_from_double(atan2(tary,tarx)+M_PI));
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t acons = trajectory_get_nearest_angle(t,cur + acible);
asserv_set_angle(t->ass,acons);
if(abs(acons - position_get_angle(t->pm)) < p->xy.prec_ap)
{
int32_t dcons = -sqrt(tarx*tarx + tary*tary);
asserv_set_distance(t->ass,dcons + position_get_distance(t->pm));
}
else
{
asserv_set_distance(t->ass,position_get_distance(t->pm));
}
}
}
else if(p->type == XY || p->type == XYPION)
{
double tarx = p->xy.x - position_get_x(t->pm);
double tary = p->xy.y - position_get_y(t->pm);
if(tarx * tarx + tary * tary < p->xy.prec_xy) {
trajectory_next(t);
}
else {
int32_t acible = position_rad2imp(t->pm,fxx_from_double(atan2(tary,tarx)));
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t acons = trajectory_get_nearest_angle(t,cur + acible);
if(t->directed_flag) {
int32_t dcons = sqrt(tarx*tarx + tary*tary);
asserv_set_angle(t->ass,acons);
asserv_set_distance(t->ass, dcons + position_get_distance(t->pm));
}
else {
asserv_set_angle(t->ass,acons);
asserv_set_distance(t->ass, position_get_distance(t->pm));
if(abs(acons - position_get_angle(t->pm)) < p->xy.prec_ap)
t->directed_flag = 1;
}
}
}
else if(p->type == XYA)
{
double tarx = p->xya.x - position_get_x(t->pm);
double tary = p->xya.y - position_get_y(t->pm);
if(tarx * tarx + tary * tary < (p->xya.prec_xy * p->xya.prec_xy)) //cété pas au carré
{
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t cons = trajectory_get_nearest_angle(t,cur + p->xya.a);
if(abs(cons - position_get_angle(t->pm)) < p->xya.prec_a)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,cons);
}
}
else
{
int32_t acible = position_rad2imp(t->pm,fxx_from_double(atan2(tary,tarx)));
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t acons = trajectory_get_nearest_angle(t,cur + acible);
asserv_set_angle(t->ass,acons);
if(abs(acons - position_get_angle(t->pm)) < p->xya.prec_ap)
{
int32_t dcons = sqrt(tarx*tarx + tary*tary);
asserv_set_distance(t->ass,dcons + position_get_distance(t->pm));
}
else
{
asserv_set_distance(t->ass,position_get_distance(t->pm));
}
}
}
else if(p->type == WAIT)
{
// asserv_set_distance(t->ass,position_get_distance(t->pm));
// asserv_set_angle(t->ass,position_get_angle(t->pm));
asserv_set_distance(t->ass,p->startingD);
asserv_set_angle(t->ass,p->startingA);
//bloutre !
}
else if(p->type == STEP)
{
// asserv_set_distance(t->ass,position_get_distance(t->pm));
// asserv_set_angle(t->ass,position_get_angle(t->pm));
asserv_set_distance(t->ass,p->startingD);
asserv_set_angle(t->ass,p->startingA);
trajectory_next(t);
printf("END STEOP\n");
//notification_notify(t->not,NOTIFICATION_TRAJECTORY_END_STEP,NULL);
}
else //step
{
trajectory_next(t);
}
}
else
{
// asserv_set_distance(t->ass,position_get_distance(t->pm));
// asserv_set_angle(t->ass,position_get_angle(t->pm));
//notification_notify(t->not,NOTIFICATION_TRAJECTORY_END_FINAL,NULL);
}
}
