/** remove event if any */
void delete_event(struct trajectory *traj)
{
set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
set_quadramp_acc(traj, traj->d_acc, traj->a_acc);
if ( traj->scheduler_task != -1) {
DEBUG(E_TRAJECTORY, "Delete event");
scheduler_del_event(traj->scheduler_task);
traj->scheduler_task = -1;
}
}
void holonomic_trajectory_manager_event(void * param)
{
///@todo : probablement des fonctions de la lib math qui font ça
struct h_trajectory *traj = (struct h_trajectory *) param;
double x = holonomic_position_get_x_double(traj->position);
double y = holonomic_position_get_y_double(traj->position);
vect2_cart vector_pos;
int32_t s_consign = 0; /**< The speed consign */
int32_t a_consign = 0; /**< The angle consign */
int32_t o_consign = 0; /**< The angular speed (omega) consign */
float target_norm = sqrtf(pow(traj->xy_target.x, 2) + pow(traj->xy_target.y, 2)); // TODO : variable never used
float position_norm = sqrtf(pow(x, 2) + pow(y, 2)); // TODO : variable never used
int32_t distance2target = sqrtf(pow(x - traj->xy_target.x, 2) + pow(y - traj->xy_target.y, 2));
vector_pos.x = x;
vector_pos.y = y;
/** @todo : move the following in the right siwtch-case */
vect2_cart vec_target = {.x = traj->xy_target.x - x,
.y = traj->xy_target.y - y};
vect2_cart vec_to_center = {.x = traj->circle_center.x - x, // TODO : variable never used
.y = traj->circle_center.y - y};
static vect2_cart keyframe = {.x = -1,
.y = -1};
/* step 1 : process new commands to quadramps */
switch (traj->moving_state)
{
case MOVING_STRAIGHT:
/* Calcul de la consigne d'angle */
a_consign = TO_DEG(vect2_angle_vec_x_rad_cart(&vec_target));
/* Calcul de la consigne de vitesse */
s_consign = SPEED_ROBOT;
if (distance2target < 250)
s_consign = 2*distance2target;
break;
case MOVING_CIRCLE:
if (keyframe.x < 0) {
keyframe.x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y,
x - traj->circle_center.x) - ANGLE_INC)*traj->radius;
keyframe.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y,
x - traj->circle_center.x)-ANGLE_INC)*traj->radius;
}
traj->xy_target.x = keyframe.x;
traj->xy_target.y = keyframe.y;
/* Calcul de la consigne d'angle */
a_consign = TO_DEG(vect2_angle_vec_x_rad_cart(&vec_target));
printf("%d\n", a_consign);
s_consign = SPEED_ROBOT/5;
//if (distance2target < 250) ///@todo : faire un truc avec holonomic_length_arc_of_circle_pnt(traj, RAD)) plutôt.
//s_consign = 2*distance2target;
break;
case MOVING_IDLE:
break;
}
switch (traj->turning_state)
{
case TURNING_CAP:
if(traj->a_target - holonomic_position_get_a_rad_double(traj->position) < 0
|| traj->a_target - holonomic_position_get_a_rad_double(traj->position) > M_PI)
o_consign = 50;
else
o_consign = -50;//holonomic_angle_2_x_rad(traj, traj->a_target);//cs_do_process(traj->csm_omega, holonomic_angle_2_x_rad(traj, ANG));
break;
case TURNING_SPEEDOFFSET:
o_consign = 1;//cs_do_process(traj->csm_omega, holonomic_angle_2_speed_rad(traj, ANG));
break;
case TURNING_FACEPOINT:
o_consign = 1;//cs_do_process(traj->csm_omega, holonomic_angle_facepoint_rad(traj, &FP));
break;
case TURNING_IDLE:
break;
}
/* step 3 : check the end of the move */
if (traj->turning_state == TURNING_IDLE && holonomic_robot_in_xy_window(traj, traj->d_win)) //@todo : not only distance, angle
{
if (traj->moving_state == MOVING_CIRCLE)
{
keyframe.x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y,
x - traj->circle_center.x) - ANGLE_INC) * traj->radius;
keyframe.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y,
x - traj->circle_center.x) - ANGLE_INC) * traj->radius;
printf("x: %f y: %f\n", keyframe.x, keyframe.y);
// If we have finished the circular trajectory
/**@todo do not work beaucause the pos has changed -> take the depaeture of the pos of restart for scratch */
vect2_cart arrival = { .x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y,
x - traj->circle_center.x) - traj->arc_angle) * traj->radius,
.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y,
x - traj->circle_center.x) - traj->arc_angle) * traj->radius};
printf("Arrival : x: %f y: %f\n", arrival.x, arrival.y);
if (vect2_dist_cart(&arrival, &vector_pos) < traj->d_win)
{
holonomic_delete_event(traj);
}
return;
}
if (prev_speed < 20)
{
traj->moving_state = MOVING_IDLE;
holonomic_delete_event(traj);
return;
}
else
s_consign = 0;
}
if (traj->moving_state == MOVING_IDLE && holonomic_robot_in_angle_window(traj, traj->a_win))
{
traj->turning_state = TURNING_IDLE;
holonomic_delete_event(traj);
return;
}
/* step 2 : pass the consign to rsh */
set_consigns_to_rsh(traj, s_consign, a_consign, o_consign);
/** @todo : re-init le keyframe !!! */
}
/** near the target (dist in x,y) ? */
uint8_t holonomic_robot_in_xy_window(struct h_trajectory *traj, double d_win)
{
vect2_cart vcp = {.x = holonomic_position_get_x_double(traj->position),
.y = holonomic_position_get_y_double(traj->position)};
return (vect2_dist_cart(&vcp, &traj->xy_target) < d_win);
}
/** returns true if the robot is in an area enclosed by a certain angle */
uint8_t holonomic_robot_in_angle_window(struct h_trajectory *traj, double a_win_rad)
{
double d_a = traj->a_target - holonomic_position_get_a_rad_double(traj->position);
if (ABS(d_a) < M_PI) {
return (ABS(d_a) < (a_win_rad/2));
} else {
return ((2 * M_PI-ABS(d_a)) < (a_win_rad/2));
}
}
/** remove event if any @todo */
void holonomic_delete_event(struct h_trajectory *traj)
{
prev_speed = 0;
traj->end_of_traj = 1;
set_consigns_to_rsh(traj, 0, holonomic_position_get_theta_v_int(traj->position), 0);
/** do not work without this : */
rsh_set_speed(traj->robot,0);
if ( traj->scheduler_task != -1) {
DEBUG(E_TRAJECTORY, "Delete event");
scheduler_del_event(traj->scheduler_task);
traj->scheduler_task = -1;
}
}
/** schedule the trajectory event */
void holonomic_schedule_event(struct h_trajectory *traj)
{
if ( traj->scheduler_task != -1) {
DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
}
else {
traj->scheduler_task =
scheduler_add_periodical_event_priority(&holonomic_trajectory_manager_event,
(void*)traj,
TRAJ_EVT_PERIOD, 30);
}
}
/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
double holonomic_simple_modulo_2pi(double a)
{
if (a < -M_PI) {
a += M_PI_2;
}
else if (a > M_PI) {
a -= M_PI_2;
}
return a;
}
/** do a modulo 2.pi -> [-Pi,+Pi] */
double holonomic_modulo_2pi(double a)
{
double res = a - (((int32_t) (a/M_PI_2)) * M_PI_2);
return holonomic_simple_modulo_2pi(res);
}
void supp(void * nothing)
{
scheduler_del_event(event_id);
}
void log_event(struct error * e, ...)
{
PGM_P txt_sev;
uint16_t flags;
va_list ap;
time_h tv;
if( e->severity > log_level)
return;
// save flags
flags = stdout->flags;
va_start(ap, e);
tv = time_get_time();
// print timestamp
printf_P(PSTR("%ld.%3.3ld | "), tv.s, (tv.us/1000UL));
// print severity
switch(e->severity)
{
case ERROR_SEVERITY_EMERG : txt_sev = PSTR(ERROR_SEVTEXT_EMERG); break;
case ERROR_SEVERITY_ERROR : txt_sev = PSTR(ERROR_SEVTEXT_ERROR); break;
case ERROR_SEVERITY_WARNING : txt_sev = PSTR(ERROR_SEVTEXT_WARNING); break;
case ERROR_SEVERITY_NOTICE : txt_sev = PSTR(ERROR_SEVTEXT_NOTICE); break;
case ERROR_SEVERITY_DEBUG : txt_sev = PSTR(ERROR_SEVTEXT_DEBUG); break;
default : txt_sev = PSTR("XXX"); break;
}
printf_P(txt_sev);
printf_P(PSTR(": "));
// print message
vfprintf_P(stdout,e->text,ap);
printf_P(PSTR("\n"));
va_end(ap);
// restore flags
stdout->flags = flags;
// dead end
if( (e->severity == ERROR_SEVERITY_ERROR)
||(e->severity == ERROR_SEVERITY_EMERG) )
{
printf_P(PSTR("\nprogram stopped, strike a key other than 'x' to reset\n"));
#if 0 //TODO:temp
//XXX Add shutdown procedures here XXX
// breaking motors
motor_cs_break(1);
// killing cs & position tasks
scheduler_del_event(event_cs);
// wait for key
uint8_t key;
while(1)
{
key = cli_getkey();
if( (key == -1)||(key == 'x'))
continue;
break;
}
#endif
uart_recv(1);
// reset MCU
reset();
}
}
