/** 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(); } }