From: zer0 <zer0@carbon.local>
Date: Wed, 5 May 2010 18:45:32 +0000 (+0200)
Subject: new base mech plate
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=092caa88280f71fb58e5938114304fe2e94d6855;p=aversive.git

new base mech plate
---

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;