new base mech plate

diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c
index c6a685b..bf97946 100644 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (executable)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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;