}
/** set speed consign */
-void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
{
uint8_t flags;
IRQ_LOCK(flags);
IRQ_UNLOCK(flags);
}
-
-
+/** set corrective coef for circle */
+void trajectory_set_circle_coef(struct trajectory *traj, double coef)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ traj->circle_coef = coef ;
+ IRQ_UNLOCK(flags);
+}
double a_win_rad; /**<< angle window (for END_NEAR) */
double a_start_rad;/**<< in xy consigns, start to move in distance
* when a_target < a_start */
-
+ double circle_coef;/**<< corrective circle coef */
+
uint16_t d_speed; /**<< distance speed consign */
uint16_t a_speed; /**<< angle speed consign */
struct robot_system *robot; /**<< associated robot_system */
struct cs *csm_angle; /**<< associated control system (angle) */
struct cs *csm_distance; /**<< associated control system (distance) */
-
+
int8_t scheduler_task; /**<< id of current task (-1 if no running task) */
};
void trajectory_set_windows(struct trajectory *traj, double d_win,
double a_win_deg, double a_start_deg);
+/**
+ * Set coef for circle trajectory. The objective of this value is to
+ * fix the radius of the circle which is not correctly what we asked.
+ */
+void trajectory_set_circle_coef(struct trajectory *traj, double coef);
+
/** return true if the position consign is equal to the filtered
* position consign (after quadramp filter), for angle and
* distance. */
double a = position_get_a_rad_double(traj->position);
int32_t d_consign = 0, a_consign = 0;
double angle_to_center_rad;
+ static int32_t d_prev, a_prev;
+ int32_t d_speed, a_speed;
+ int32_t d_pos, a_pos;
+
+ d_pos = rs_get_distance(traj->robot);
+ a_pos = rs_get_angle(traj->robot);
+ d_speed = d_pos - d_prev;
+ a_speed = a_pos - a_prev;
+ d_prev = d_pos;
+ a_prev = a_pos;
/* These vectors contain target position of the robot in
* its own coordinates */
vect2_cart v2cart_pos;
vect2_pol v2pol_target;
+ int32_t delta_d, delta_a;
+ double coef_deriv = traj->circle_coef;
+ double new_radius;
+ struct quadramp_filter * q_d, * q_a;
+
+ q_d = traj->csm_distance->consign_filter_params;
+ q_a = traj->csm_angle->consign_filter_params;
+ delta_a = a_speed;//q_a->previous_var;
+ delta_d = d_speed;//q_d->previous_var;
+
/* step 1 : process new commands to quadramps */
/* process the command vector from current position to the
vect2_cart2pol(&v2cart_pos, &v2pol_target);
v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
+ /* pas trop mal, mais oscille */
+ //new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv;
+ if (v2pol_target.r > traj->target.circle.radius/2)
+ new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r;
+ else
+ new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ;
+
+ /* oscille a mort */
+ //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * delta_a * coef_deriv;
+
+ /* ? */
+ //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * coef_deriv;
+
+
/* wanted direction of center of circle:
* if we are far, go in the center direction,
* if we are at radius, we want to see the center at 90°
* if we are nearer than radius, angle to center is > 90° */
- if (v2pol_target.r > traj->target.circle.radius) {
- angle_to_center_rad = traj->target.circle.radius / v2pol_target.r;
+ if (v2pol_target.r > new_radius) {
+ angle_to_center_rad = new_radius / v2pol_target.r;
angle_to_center_rad *= (M_PI / 2);
}
else {
angle_to_center_rad = 1. - (v2pol_target.r /
- (2 * traj->target.circle.radius));
+ (2 * new_radius));
angle_to_center_rad *= M_PI;
}
EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32
", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32
- ", a_speed=%" PRIi32,
- rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
- rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
+ ", a_speed=%" PRIi32 "radius = %f",
+ rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
+ rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj),
+ new_radius);
cs_set_consign(traj->csm_angle, a_consign);
cs_set_consign(traj->csm_distance, d_consign);
extern parse_pgm_inst_t cmd_traj_speed_show;
extern parse_pgm_inst_t cmd_trajectory;
extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_circle_coef;
+extern parse_pgm_inst_t cmd_circle_coef_show;
extern parse_pgm_inst_t cmd_rs_gains;
extern parse_pgm_inst_t cmd_rs_gains_show;
extern parse_pgm_inst_t cmd_track;
(parse_pgm_inst_t *)&cmd_traj_speed_show,
(parse_pgm_inst_t *)&cmd_trajectory,
(parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_circle_coef,
+ (parse_pgm_inst_t *)&cmd_circle_coef_show,
(parse_pgm_inst_t *)&cmd_rs_gains,
(parse_pgm_inst_t *)&cmd_rs_gains_show,
(parse_pgm_inst_t *)&cmd_track,
},
};
+/**********************************************************/
+/* circle coef configuration */
+
+/* this structure is filled when cmd_circle_coef is parsed successfully */
+struct cmd_circle_coef_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float circle_coef;
+};
+
+
+/* function called when cmd_circle_coef is parsed successfully */
+static void cmd_circle_coef_parsed(void *parsed_result, void *data)
+{
+ struct cmd_circle_coef_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
+ }
+
+ printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
+}
+
+prog_char str_circle_coef_arg0[] = "circle_coef";
+parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
+prog_char str_circle_coef_arg1[] = "set";
+parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
+parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
+
+prog_char help_circle_coef[] = "Set circle coef";
+parse_pgm_inst_t cmd_circle_coef = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_arg1,
+ (prog_void *)&cmd_circle_coef_val,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_circle_coef_show_arg[] = "show";
+parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
+
+prog_char help_circle_coef_show[] = "Show circle coef";
+parse_pgm_inst_t cmd_circle_coef_show = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_show_arg,
+ NULL,
+ },
+};
+
/**********************************************************/
/* trajectory window configuration */
import shlex
import time
import math
+import re
import warnings
warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
time.sleep(1)
self.ser.write("pwm s3(3C) 250\n")
+ def do_circle(self, arg):
+ arg = re.sub(" *", " ", arg)
+ arg = arg.strip()
+ arg = arg.split(" ")
+ if len(arg)!=4:
+ print "radius coef dspeed aspeed"
+ return
+ r = int(arg[0])
+ c = float(arg[1])
+ dspeed = int(arg[2])
+ aspeed = int(arg[3])
+
+ self.ser.write("event cs on\n")
+ time.sleep(0.3)
+
+ self.ser.write("traj_speed distance %d\n"%dspeed)
+ time.sleep(0.3)
+ self.ser.write("traj_speed angle %d\n"%aspeed)
+ time.sleep(0.3)
+
+ self.ser.write("goto d_rel 300\n")
+ time.sleep(3)
+
+ self.ser.write("position reset\n")
+ time.sleep(0.3)
+ self.ser.write("circle_coef set %f\n"%c)
+ time.sleep(0.3)
+
+ self.ser.write("goto circle_rel 700 460 %d 360\n"%r)
+ self.ser.flushInput()
+ time.sleep(0.3)
+
+
if __name__ == "__main__":
try:
import readline,atexit
extern parse_pgm_inst_t cmd_traj_speed_show;
extern parse_pgm_inst_t cmd_trajectory;
extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_circle_coef;
+extern parse_pgm_inst_t cmd_circle_coef_show;
extern parse_pgm_inst_t cmd_rs_gains;
extern parse_pgm_inst_t cmd_rs_gains_show;
extern parse_pgm_inst_t cmd_track;
(parse_pgm_inst_t *)&cmd_traj_speed_show,
(parse_pgm_inst_t *)&cmd_trajectory,
(parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_circle_coef,
+ (parse_pgm_inst_t *)&cmd_circle_coef_show,
(parse_pgm_inst_t *)&cmd_rs_gains,
(parse_pgm_inst_t *)&cmd_rs_gains_show,
(parse_pgm_inst_t *)&cmd_track,
},
};
+/**********************************************************/
+/* circle coef configuration */
+
+/* this structure is filled when cmd_circle_coef is parsed successfully */
+struct cmd_circle_coef_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float circle_coef;
+};
+
+
+/* function called when cmd_circle_coef is parsed successfully */
+static void cmd_circle_coef_parsed(void *parsed_result, void *data)
+{
+ struct cmd_circle_coef_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
+ }
+
+ printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
+}
+
+prog_char str_circle_coef_arg0[] = "circle_coef";
+parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
+prog_char str_circle_coef_arg1[] = "set";
+parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
+parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
+
+prog_char help_circle_coef[] = "Set circle coef";
+parse_pgm_inst_t cmd_circle_coef = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_arg1,
+ (prog_void *)&cmd_circle_coef_val,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_circle_coef_show_arg[] = "show";
+parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
+
+prog_char help_circle_coef_show[] = "Show circle coef";
+parse_pgm_inst_t cmd_circle_coef_show = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_show_arg,
+ NULL,
+ },
+};
+
/**********************************************************/
/* trajectory window configuration */
git.droids-corp.org / aversive.git / commitdiff