From 092caa88280f71fb58e5938114304fe2e94d6855 Mon Sep 17 00:00:00 2001 From: zer0 Date: Wed, 5 May 2010 20:45:32 +0200 Subject: [PATCH] new base mech plate --- projects/microb2010/cobboard/actuator.c | 4 +- projects/microb2010/cobboard/sensor.c | 4 +- projects/microb2010/cobboard/sensor.h | 4 +- projects/microb2010/cobboard/shovel.c | 2 +- projects/microb2010/cobboard/spickle.c | 4 +- projects/microb2010/mainboard/commands_traj.c | 13 ++++-- projects/microb2010/mainboard/cs.c | 46 +++---------------- projects/microb2010/mainboard/display.py | 35 +++++++------- projects/microb2010/mainboard/i2c_protocol.c | 7 ++- projects/microb2010/mainboard/main.h | 6 +-- projects/microb2010/mainboard/robotsim.c | 23 +++++++--- projects/microb2010/mainboard/robotsim.h | 2 +- projects/microb2010/mainboard/strat.c | 9 ++-- projects/microb2010/mainboard/strat.h | 4 +- projects/microb2010/mainboard/strat_avoid.c | 5 ++ projects/microb2010/mainboard/strat_corn.c | 8 +--- 16 files changed, 80 insertions(+), 96 deletions(-) diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index c6a685b..bf97946 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -48,8 +48,8 @@ #define COBROLLER_SPEED 1000 #define SERVO_DOOR_OPEN 300 -#define SERVO_DOOR_CLOSED 530 -#define SERVO_DOOR_BLOCK 530 +#define SERVO_DOOR_CLOSED 525 +#define SERVO_DOOR_BLOCK 525 #define SERVO_CARRY_L_OPEN 295 #define SERVO_CARRY_L_CLOSED 430 //400 diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index eb1f42e..b6c3d34 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -143,9 +143,9 @@ struct sensor_filter { * CAP 1,5,6,7,8 (notation elec) */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { - [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 1 }, /* 0 */ + [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 1 }, /* 0 */ [S_CAP2] = { 10, 0, 3, 7, 0, 0 }, /* 1 */ - [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ + [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ [S_CAP6] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index a603e97..5a84e5b 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -28,9 +28,9 @@ #define ADC_MAX 4 /* synchronize with sensor.c */ -#define S_COB_INSIDE_L 0 +#define S_COB_INSIDE_R 0 #define S_CAP2 1 -#define S_COB_INSIDE_R 2 +#define S_COB_INSIDE_L 2 #define S_CAP4 3 #define S_LCOB 4 #define S_CAP6 5 diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index b2a8cc0..bd0c7c9 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -44,7 +44,7 @@ #include "shovel.h" #define SHOVEL_DOWN 100 -#define SHOVEL_MID 4500 +#define SHOVEL_MID 5250 #define SHOVEL_UP 11000 /* init spickle position at beginning */ diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 7296d8e..a6bf6db 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -67,8 +67,8 @@ static struct spickle_params spickle = { &cobboard.right_spickle, }, .pos_deployed = { - 7000, // 200, /* left */ - 7000, // 200, /* right */ + 500,// 7000, // 200, /* left */ + 500,// 7000, // 200, /* right */ }, .pos_mid = { 25000, /* left */ diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index e519d17..92ab9a0 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -823,6 +823,7 @@ static void auto_position(void) interrupt_traj_reset(); strat_get_speed(&old_spdd, &old_spda); strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); + strat_set_acc(3, 3); err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) @@ -832,16 +833,18 @@ static void auto_position(void) COLOR_A(-90)); strat_hardstop(); - trajectory_d_rel(&mainboard.traj, -180); + trajectory_d_rel(&mainboard.traj, -70); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; + time_wait_ms(250); trajectory_a_rel(&mainboard.traj, COLOR_A(-90)); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; + time_wait_ms(250); err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; @@ -850,17 +853,17 @@ static void auto_position(void) 180); strat_hardstop(); - trajectory_d_rel(&mainboard.traj, -170); + trajectory_d_rel(&mainboard.traj, -80); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - wait_ms(100); + time_wait_ms(250); - trajectory_a_rel(&mainboard.traj, COLOR_A(-110)); + trajectory_a_rel(&mainboard.traj, COLOR_A(-135)); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - wait_ms(100); + time_wait_ms(250); strat_set_speed(old_spdd, old_spda); return; diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 4e4d298..896fa16 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -80,15 +80,6 @@ int32_t encoders_right_cobroller_speed(void *number) } #endif -#ifndef HOST_VERSION -#define DEBUG_CPLD -#endif - -#ifdef DEBUG_CPLD -extern int16_t g_encoders_spi_previous[4]; -static int32_t ll_prev, rr_prev; -#endif - /* called every 5 ms */ static void do_cs(void *dummy) { @@ -98,29 +89,10 @@ static void do_cs(void *dummy) #ifdef HOST_VERSION robotsim_update(); #else -#ifdef DEBUG_CPLD - int32_t ll, rr; -#endif /* read encoders */ if (mainboard.flags & DO_ENCODERS) { encoders_spi_manage(NULL); } -#ifdef DEBUG_CPLD - ll = encoders_spi_get_value(LEFT_ENCODER); - rr = encoders_spi_get_value(RIGHT_ENCODER); - if ((ll - ll_prev > 3000) || (ll - ll_prev < -3000) || - (rr - rr_prev > 3000) || (rr - rr_prev < -3000)) { - printf_P(PSTR("/ %d %d %d %d\r\n"), - g_encoders_spi_previous[0], - g_encoders_spi_previous[1], - g_encoders_spi_previous[2], - g_encoders_spi_previous[3]); - BRAKE_ON(); - while (1); - } - ll_prev = ll; - rr_prev = rr; -#endif #endif /* robot system, conversion to angle,distance */ @@ -161,11 +133,11 @@ static void do_cs(void *dummy) bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs); bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs); if (mainboard.flags & DO_ERRBLOCKING) { - if (bd_get(&mainboard.left_cobroller.bd) || - bd_get(&mainboard.left_cobroller.bd)) { - printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); - mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); - } +/* if (bd_get(&mainboard.left_cobroller.bd) || */ +/* bd_get(&mainboard.left_cobroller.bd)) { */ +/* printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); */ +/* mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); */ +/* } */ } #endif } @@ -193,7 +165,6 @@ static void do_cs(void *dummy) #ifdef HOST_VERSION if ((cpt & 7) == 0) { - // dump_cs("dist", &mainboard.distance.cs); robotsim_dump(); } #endif @@ -252,7 +223,7 @@ void microb_cs_init(void) position_init(&mainboard.pos); position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); position_set_related_robot_system(&mainboard.pos, &mainboard.rs); - position_set_centrifugal_coef(&mainboard.pos, 0.000025); + //position_set_centrifugal_coef(&mainboard.pos, 0.000025); position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ @@ -365,11 +336,6 @@ void microb_cs_init(void) mainboard.left_cobroller.on = 1; mainboard.right_cobroller.on = 1; -#ifdef DEBUG_CPLD - ll_prev = encoders_spi_get_value(LEFT_ENCODER); - rr_prev = encoders_spi_get_value(RIGHT_ENCODER); -#endif - scheduler_add_periodical_event_priority(do_cs, NULL, 5000L / SCHEDULER_UNIT, CS_PRIO); diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 19ff022..f89580c 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -10,6 +10,9 @@ AREA_Y = 2100. ROBOT_HEIGHT=5 # 350 CORN_HEIGHT=5 # 150 +ROBOT_WIDTH=320 +ROBOT_LENGTH=360 + area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ] areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area) area_box = box(size=areasize, color=(0.0, 1.0, 0.0)) @@ -276,21 +279,21 @@ def set_robot(): 0) robot.axis = axis - robot.size = (250, 320, ROBOT_HEIGHT) + robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT) robot_lspickle = 2 # XXX lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180), tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180), ROBOT_HEIGHT/2) lspickle.axis = axis - lspickle.size = (20, 320, 5) + lspickle.size = (20, ROBOT_WIDTH, 5) robot_rspickle = 2 # XXX rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180), tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180), ROBOT_HEIGHT/2) rspickle.axis = axis - rspickle.size = (20, 320, 5) + rspickle.size = (20, ROBOT_WIDTH, 5) # save position save_pos.append((robot.pos.x, robot.pos.y, tmp_a)) @@ -351,21 +354,21 @@ while True: # parse cobboard if not m: - m = re.match("cobboard=%s"%(INT), l) + m = re.match("cobboard=%s,%s"%(INT,INT), l) if m: - mode = int(m.groups()[0]) - if (mode & 0x01) == 0: - robot_lspickle = 0 - elif (mode & 0x02) == 0: - robot_lspickle = 1 - else: - robot_lspickle = 2 - if (mode & 0x04) == 0: - robot_rspickle = 0 - elif (mode & 0x08) == 0: - robot_rspickle = 1 + print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1])) + side = int(m.groups()[0]) + flags = int(m.groups()[1]) + if side == 0: + if (flags & 0x01) == 0: + robot_lspickle = 1 + else: + robot_lspickle = 2 else: - robot_rspickle = 2 + if (flags & 0x01) == 0: + robot_rspickle = 1 + else: + robot_rspickle = 2 if scene.kb.keys == 0: continue diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 7a2c378..3354e39 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -403,7 +403,8 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state) int8_t i2c_cobboard_set_mode(uint8_t mode) { #ifdef HOST_VERSION - return robotsim_i2c_cobboard_set_mode(mode); + cobboard.mode = mode; + return 0; #else struct i2c_cmd_cobboard_set_mode buf; buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; @@ -414,11 +415,15 @@ int8_t i2c_cobboard_set_mode(uint8_t mode) static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags) { +#ifdef HOST_VERSION + return robotsim_i2c_cobboard_set_spickles(side, flags); +#else if (side == I2C_LEFT_SIDE) cobboard.lspickle = flags; else cobboard.rspickle = flags; return 0; +#endif } int8_t i2c_cobboard_pack(uint8_t side) diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 00b6e86..0798699 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -76,11 +76,11 @@ #define MATCH_TIME 89 /* decrease track to decrease angle */ -#define EXT_TRACK_MM 304.61875 +#define EXT_TRACK_MM 304.9 #define VIRTUAL_TRACK_MM EXT_TRACK_MM -#define ROBOT_HALF_LENGTH_FRONT 130 -#define ROBOT_HALF_LENGTH_REAR 120 +#define ROBOT_HALF_LENGTH_FRONT 180 +#define ROBOT_HALF_LENGTH_REAR 70 #define ROBOT_WIDTH 320 /* it is a 1024 imps -> 4096 because we see 1/4 period diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 2d0d7be..7c52835 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -111,16 +111,25 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd) } int8_t -robotsim_i2c_cobboard_set_mode(uint8_t mode) +robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags) { char buf[BUFSIZ]; int len; - if (cobboard.mode == mode) - return 0; + if (side == I2C_LEFT_SIDE) { + if (cobboard.lspickle == flags) + return 0; + else + cobboard.lspickle = flags; + } + if (side == I2C_RIGHT_SIDE) { + if (cobboard.rspickle == flags) + return 0; + else + cobboard.rspickle = flags; + } - cobboard.mode = mode; - len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode); + len = snprintf(buf, sizeof(buf), "cobboard=%d,%d\n", side, flags); hostsim_lock(); write(fdw, buf, len); hostsim_unlock(); @@ -218,8 +227,8 @@ void robotsim_update(void) ((local_r_pwm * 1000 * FILTER2)/1000); /* basic collision detection */ - a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR); - d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR); + a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_FRONT); + d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_FRONT); xfl = x + cos(a+a2) * d; yfl = y + sin(a+a2) * d; diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h index 8246500..0950af0 100644 --- a/projects/microb2010/mainboard/robotsim.h +++ b/projects/microb2010/mainboard/robotsim.h @@ -25,4 +25,4 @@ void robotsim_pwm(void *arg, int32_t val); int32_t robotsim_encoder_get(void *arg); int robotsim_init(void); void robotsim_dump(void); -int8_t robotsim_i2c_cobboard_set_mode(uint8_t mode); +int8_t robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 0405844..db2221f 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -116,8 +116,8 @@ void strat_event_disable(void) void strat_init(void) { #ifdef HOST_VERSION - position_set(&mainboard.pos, 298.16, - COLOR_Y(308.78), COLOR_A(70.00)); + position_set(&mainboard.pos, 258., + COLOR_Y(246.), COLOR_A(45.)); #endif /* we consider that the color is correctly set */ @@ -350,10 +350,9 @@ static uint8_t strat_beginning(void) uint8_t err; strat_set_acc(ACC_DIST, ACC_ANGLE); - strat_set_speed(600, 60); /* OK */ - //strat_set_speed(250, 28); /* OK */ + strat_set_speed(400, 150); /* OK */ - trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); + trajectory_d_a_rel(&mainboard.traj, 800, COLOR_A(45)); err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_STD); diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 0930706..8a2de56 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -142,8 +142,8 @@ #define SPEED_DIST_VERY_SLOW 400. #define SPEED_ANGLE_VERY_SLOW 400. -#define SPEED_CLITOID_SLOW 250. -#define SPEED_CLITOID_FAST 500. +#define SPEED_CLITOID_SLOW 100. ///250. +#define SPEED_CLITOID_FAST 100. ///500. /* strat infos structures */ diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 61151ff..4f497ea 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -898,6 +898,11 @@ uint8_t strat_harvest_circuit(void) return END_TRAJ; // XXX } +uint8_t strat_unblock(void) +{ + return END_TRAJ; +} + void test_strat_avoid(void) { #ifdef TEST_STRAT_AVOID diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 48eff29..74dd8d1 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -315,7 +315,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1, } /* hard turn, 120 deg */ else { - radius = 75; + radius = 120;//75; if (diff_a_deg > 0) beta_deg = 0; else @@ -381,12 +381,6 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, DEBUG(E_USER_STRAT, "clitoid finished"); - /* XXX 600 -> cste */ - /* XXX does not work, do better */ -/* if (err == 0 && d_speed < 600 && */ -/* mainboard.traj.state == RUNNING_CLITOID_LINE) */ -/* strat_set_speed(600, SPEED_ANGLE_FAST); */ - strat_rpack60 = 0; strat_lpack60 = 0; return err; -- 2.20.1