-/*
+/*
* Copyright Droids Corporation, Microb Technology, Eirbot (2005)
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
#include "robot_system.h"
-/** Call a pwm() pointer :
+/** Call a pwm() pointer :
* - lock the interrupts
* - read the pointer to the pwm function
* - unlock the interrupts
}
}
-/** Call a encoder() pointer :
+/** Call a encoder() pointer :
* - lock the interrupts
* - read the pointer to the encoder function
* - unlock the interrupts
#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/** define left motor encoder function and param */
-void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *),
+void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *),
void *left_mot_encoder_param, double gain)
{
uint8_t flags;
}
/** define right motor encoder function and param */
-void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *),
+void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *),
void *right_mot_encoder_param, double gain)
{
uint8_t flags;
#endif
/** define left external encoder function and param */
-void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *),
+void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *),
void *left_ext_encoder_param, double gain)
{
uint8_t flags;
}
/** define right external encoder function and param */
-void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *),
+void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *),
void *right_ext_encoder_param, double gain)
{
uint8_t flags;
/**** Virtual encoders and PWM */
-/**
+/**
* set the real pwms according to the specified angle (it also
- * depends on the last distance command sent)
+ * depends on the last distance command sent)
*/
void rs_set_angle(void * data, int32_t angle)
{
p.angle = angle;
rs_get_wheels_from_polar(&w, &p);
-
+
safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
}
-/**
+/**
* set the real pwms according to the specified distance (it also
- * depends on the last angle command sent)
+ * depends on the last angle command sent)
*/
void rs_set_distance(void * data, int32_t distance)
{
p.distance = distance;
rs_get_wheels_from_polar(&w, &p);
-
+
safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
}
-/**
- * get the virtual angle according to real encoders value.
+/**
+ * get the virtual angle according to real encoders value.
*/
int32_t rs_get_angle(void * data)
{
struct robot_system * rs = data;
int32_t angle;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- angle = rs->virtual_encoders.angle ;
+ angle = rs->virtual_encoders.angle ;
IRQ_UNLOCK(flags);
return angle;
}
-/**
- * get the virtual distance according to real encoders value.
+/**
+ * get the virtual distance according to real encoders value.
*/
int32_t rs_get_distance(void * data)
{
struct robot_system * rs = data;
int32_t distance;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- distance = rs->virtual_encoders.distance ;
+ distance = rs->virtual_encoders.distance ;
IRQ_UNLOCK(flags);
return distance;
}
struct robot_system * rs = data;
int32_t angle;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- angle = rs->pext_prev.angle ;
+ angle = rs->pext_prev.angle ;
IRQ_UNLOCK(flags);
return angle;
}
struct robot_system * rs = data;
int32_t distance;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- distance = rs->pext_prev.distance ;
+ distance = rs->pext_prev.distance ;
IRQ_UNLOCK(flags);
return distance;
}
{
struct robot_system * rs = data;
int32_t angle;
- uint8_t flags;
+ uint8_t flags;
IRQ_LOCK(flags);
- angle = rs->pmot_prev.angle ;
+ angle = rs->pmot_prev.angle ;
IRQ_UNLOCK(flags);
return angle;
}
struct robot_system * rs = data;
int32_t distance;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- distance = rs->pmot_prev.distance ;
+ distance = rs->pmot_prev.distance ;
IRQ_UNLOCK(flags);
return distance;
}
struct robot_system * rs = data;
int32_t left;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- left = rs->wext_prev.left ;
+ left = rs->wext_prev.left ;
IRQ_UNLOCK(flags);
return left;
}
struct robot_system * rs = data;
int32_t right;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- right = rs->wext_prev.right ;
+ right = rs->wext_prev.right ;
IRQ_UNLOCK(flags);
return right;
}
{
struct robot_system * rs = data;
int32_t left;
- uint8_t flags;
+ uint8_t flags;
IRQ_LOCK(flags);
- left = rs->wmot_prev.left ;
+ left = rs->wmot_prev.left ;
IRQ_UNLOCK(flags);
return left;
}
struct robot_system * rs = data;
int32_t right;
uint8_t flags;
-
+
IRQ_LOCK(flags);
- right = rs->wmot_prev.right ;
+ right = rs->wmot_prev.right ;
IRQ_UNLOCK(flags);
return right;
}
void rs_set_flags(struct robot_system * rs, uint8_t flags)
{
uint8_t i_flags;
-
+
IRQ_LOCK(i_flags);
rs->flags = flags;
IRQ_UNLOCK(i_flags);
}
-/**
+/**
* Read the encoders, and update internal virtual counters. Call this
* function is needed before reading the virtual encoders.The program
* will decide if it the external encoders or the motor encoders are
#endif
int32_t delta_angle, delta_distance;
uint8_t flags;
-
+
/* read encoders */
wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param);
wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param);
wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param);
wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param);
#endif
-
+
/* apply gains to each wheel */
if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
#ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
void trajectory_circle_rel(struct trajectory *traj, double x, double y,
double radius_mm, double rel_a_deg, uint8_t flags);
+/*
+ * Compute the fastest distance and angle speeds matching the radius
+ * from current traj_speed
+ */
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+ double radius_mm,
+ double *speed_d,
+ double *speed_a);
+
/* do a line */
void trajectory_line_abs(struct trajectory *traj, double x1, double y1,
double x2, double y2, double advance);
* Compute the fastest distance and angle speeds matching the radius
* from current traj_speed
*/
-/* static */void circle_get_da_speed_from_radius(struct trajectory *traj,
- double radius_mm,
- double *speed_d,
- double *speed_a)
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+ double radius_mm,
+ double *speed_d,
+ double *speed_a)
{
/* speed_d = coef * speed_a */
double coef;
SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
SRC += strat_utils.c strat_base.c strat.c
-ifneq ($(HOST),avr)
+ifeq ($(H),1)
SRC += robotsim.c
endif
ASRC =
CFLAGS += -Wall -Werror
-ifeq ($(HOST),avr)
+ifneq ($(H),1)
LDFLAGS = -T ../common/avr6.x
endif
struct cmd_test_result {
fixed_string_t arg0;
int32_t radius;
+ int32_t dist;
};
+/* function called when cmd_test is parsed successfully */
+static void line2line(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist)
+{
+ uint8_t err;
+ int32_t dist_imp_target;
+ double speed_d, speed_a;
+ double distance, angle;
+ double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
+ double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
+
+ printf("%s()\n", __FUNCTION__);
+ strat_set_speed(500, 500);
+ circle_get_da_speed_from_radius(&mainboard.traj, radius,
+ &speed_d, &speed_a);
+ trajectory_line_abs(&mainboard.traj,
+ line1x1, line1y1,
+ line1x2, line1y2, 150.);
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+ dist, TRAJ_FLAGS_NO_NEAR);
+ /* circle */
+ strat_set_speed(speed_d, speed_a);
+ angle = line2_angle - line1_angle;
+ distance = angle * radius;
+ if (distance < 0)
+ distance = -distance;
+ dist_imp_target = rs_get_distance(&mainboard.rs) +
+ distance * mainboard.pos.phys.distance_imp_per_mm;
+ angle = DEG(angle);
+ distance += 100; /* take some margin to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance, angle);
+
+ err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target,
+ TRAJ_FLAGS_NO_NEAR);
+
+ strat_set_speed(500, 500);
+ trajectory_line_abs(&mainboard.traj,
+ line2x1, line2y1,
+ line2x2, line2y2, 150.);
+}
+
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
+ // struct cmd_test_result *res = parsed_result;
+#ifdef HOST_VERSION
+ strat_reset_pos(400, 400, 90);
+ mainboard.angle.on = 1;
+ mainboard.distance.on = 1;
+#endif
+ line2line(375, 347, 375, 1847,
+ 375, 1847, 1050, 1472,
+ 100, 200);
+ line2line(825, 1596, 1050, 1472,
+ 1050, 1472, 1500, 1722,
+ 180, 120);
+ line2line(1050, 1472, 1500, 1722,
+ 1500, 1722, 2175, 1347,
+ 180, 120);
+ line2line(1500, 1722, 2175, 1347,
+ 2175, 1347, 2175, 847,
+ 150, 120);
+ line2line(2175, 1347, 2175, 847,
+ 2175, 847, 2400, 722,
+ 150, 120);
+ line2line(2175, 847, 2400, 722,
+ 2400, 722, 2625, 847,
+ 150, 100);
+ line2line(2400, 722, 2625, 847,
+ 2625, 847, 2625, 1847,
+ 150, 100);
+ line2line(2625, 847, 2625, 1847,
+ 2625, 1847, 375, 597,
+ 100, 200);
}
prog_char str_test_arg0[] = "test";
parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
+parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
prog_char help_test[] = "Test function";
parse_pgm_inst_t cmd_test = {
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
(prog_void *)&cmd_test_arg0,
- (prog_void *)&cmd_test_arg1,
NULL,
},
};
robot = box(pos = (0, 0, 150),
size = (250,320,350),
- color = (1, 0, 0) )
+ color = (0.3, 0.3, 0.3) )
last_pos = robot.pos.x, robot.pos.y, robot.pos.z
hcenter_line = curve()
vcenter_line = curve()
vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
+yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
+yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
+
+bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
+bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
+
+greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
+greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
+greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
+
def square(sz):
sq = curve()
sq.pos = [(-sz, -sz, 0.3),
TYPE_WHITE_CORN=2
TYPE_BLACK_CORN=3
TYPE_OBSTACLE=4
-TYPE_NEIGH=5
+TYPE_BALL=5
+TYPE_NEIGH=6
col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
if c >= 0:
waypoints[i][j] = corn_table[c]
continue
+
+ # balls
+ if (i & 1) == 0 and j > 3:
+ waypoints[i][j] = TYPE_BALL
+ continue
+ if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
+ waypoints[i][j] = TYPE_BALL
+ continue
+
# too close of border
if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
waypoints[i][j] = TYPE_OBSTACLE
radius=25, color=(0.2,0.2,0.2),
pos=(x-AREA_X/2,y-AREA_Y/2,75))
area_objects.append(c)
+ elif waypoints[i][j] == TYPE_BALL:
+ c = sphere(radius=50, color=(1., 0.,0.),
+ pos=(x-AREA_X/2,y-AREA_Y/2,50))
+ area_objects.append(c)
+ else:
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(x-AREA_X/2,y-AREA_Y/2,5))
+ area_objects.append(c)
j += 1
y += STEP_CORN_Y
i += 1
git.droids-corp.org / aversive.git / commitdiff