From: zer0 <zer0@carbon.local>
Date: Wed, 5 May 2010 23:13:44 +0000 (+0200)
Subject: current limit on shovel
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=f5502f60124c62801685777bb7c699bdfb7e1166

current limit on shovel
---

diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c
index feeb32f..df0b701 100644
--- a/projects/microb2010/cobboard/commands.c
+++ b/projects/microb2010/cobboard/commands.c
@@ -79,6 +79,8 @@ extern parse_pgm_inst_t cmd_spickle_params_show;
 extern parse_pgm_inst_t cmd_spickle_params2;
 extern parse_pgm_inst_t cmd_spickle_params2_show;
 extern parse_pgm_inst_t cmd_shovel;
+extern parse_pgm_inst_t cmd_shovel_current;
+extern parse_pgm_inst_t cmd_shovel_current_show;
 extern parse_pgm_inst_t cmd_test;
 
 
@@ -140,6 +142,8 @@ parse_pgm_ctx_t main_ctx[] = {
 	(parse_pgm_inst_t *)&cmd_spickle_params2,
 	(parse_pgm_inst_t *)&cmd_spickle_params2_show,
 	(parse_pgm_inst_t *)&cmd_shovel,
+	(parse_pgm_inst_t *)&cmd_shovel_current,
+	(parse_pgm_inst_t *)&cmd_shovel_current_show,
 	(parse_pgm_inst_t *)&cmd_test,
 
 	NULL,
diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index 669c4c8..dc1ec35 100644
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -759,6 +759,79 @@ parse_pgm_inst_t cmd_spickle_params2_show = {
 	},
 };
 
+/**********************************************************/
+/* Set Shovel Params */
+
+/* this structure is filled when cmd_shovel_current is parsed successfully */
+struct cmd_shovel_current_result {
+	fixed_string_t arg0;
+	fixed_string_t arg1;
+	int32_t arg2;
+	int32_t arg3;
+};
+
+/* function called when cmd_shovel_current is parsed successfully */
+static void cmd_shovel_current_parsed(void *parsed_result,
+				      __attribute__((unused)) void *data)
+{
+	struct cmd_shovel_current_result * res = parsed_result;
+	uint8_t enable;
+	int32_t k1, k2;
+
+	if (!strcmp_P(res->arg1, PSTR("set")))
+		shovel_set_current_limit_coefs(res->arg2, res->arg3);
+	else if (!strcmp_P(res->arg1, PSTR("on")))
+		shovel_current_limit_enable(1);
+	else if (!strcmp_P(res->arg1, PSTR("off")))
+		shovel_current_limit_enable(0);
+
+	/* else show */
+	enable = shovel_get_current_limit_coefs(&k1, &k2);
+	printf_P(PSTR("enabled=%d k1=%"PRIi32" k2=%"PRIi32"\r\n"),
+		 enable, k1, k2);
+}
+
+prog_char str_shovel_current_arg0[] = "shovel_current";
+parse_pgm_token_string_t cmd_shovel_current_arg0 =
+	TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg0, str_shovel_current_arg0);
+prog_char str_shovel_current_arg1[] = "set";
+parse_pgm_token_string_t cmd_shovel_current_arg1 =
+	TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg1, str_shovel_current_arg1);
+parse_pgm_token_num_t cmd_shovel_current_arg2 =
+	TOKEN_NUM_INITIALIZER(struct cmd_shovel_current_result, arg2, INT32);
+parse_pgm_token_num_t cmd_shovel_current_arg3 =
+	TOKEN_NUM_INITIALIZER(struct cmd_shovel_current_result, arg3, INT32);
+
+prog_char help_shovel_current[] = "Set shovel_current values";
+parse_pgm_inst_t cmd_shovel_current = {
+	.f = cmd_shovel_current_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_shovel_current,
+	.tokens = {        /* token list, NULL terminated */
+		(prog_void *)&cmd_shovel_current_arg0,
+		(prog_void *)&cmd_shovel_current_arg1,
+		(prog_void *)&cmd_shovel_current_arg2,
+		(prog_void *)&cmd_shovel_current_arg3,
+		NULL,
+	},
+};
+
+prog_char str_shovel_current_arg1_show[] = "show#on#off";
+parse_pgm_token_string_t cmd_shovel_current_arg1_show =
+	TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg1, str_shovel_current_arg1_show);
+
+prog_char help_shovel_current_show[] = "show shovel params";
+parse_pgm_inst_t cmd_shovel_current_show = {
+	.f = cmd_shovel_current_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_shovel_current_show,
+	.tokens = {        /* token list, NULL terminated */
+		(prog_void *)&cmd_shovel_current_arg0,
+		(prog_void *)&cmd_shovel_current_arg1_show,
+		NULL,
+	},
+};
+
 /**********************************************************/
 /* Test */
 
diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index f8c7123..386b360 100644
--- a/projects/microb2010/cobboard/cs.c
+++ b/projects/microb2010/cobboard/cs.c
@@ -46,6 +46,7 @@
 #include "main.h"
 #include "actuator.h"
 #include "spickle.h"
+#include "shovel.h"
 
 /* called every 5 ms */
 static void do_cs(__attribute__((unused)) void *dummy)
@@ -173,7 +174,7 @@ void microb_cs_init(void)
 	cs_init(&cobboard.shovel.cs);
 	cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
 	cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
-	cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
+	cs_set_process_in(&cobboard.shovel.cs, shovel_set, SHOVEL_PWM);
 	cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);
 	cs_set_consign(&cobboard.shovel.cs, 0);
 
diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c
index b2a8cc0..56efa96 100644
--- a/projects/microb2010/cobboard/shovel.c
+++ b/projects/microb2010/cobboard/shovel.c
@@ -47,6 +47,10 @@
 #define SHOVEL_MID  4500
 #define SHOVEL_UP   11000
 
+static int32_t shovel_k1 = 1000;
+static int32_t shovel_k2 = 20;
+static uint8_t shovel_current_limit = 1;
+
 /* init spickle position at beginning */
 static void shovel_autopos(void)
 {
@@ -69,6 +73,51 @@ static uint8_t shovel_is_at_pos(int32_t pos)
 	return 0;
 }
 
+void shovel_set_current_limit_coefs(int32_t k1, int32_t k2)
+{
+	shovel_k1 = k1;
+	shovel_k2 = k2;
+}
+
+uint8_t shovel_get_current_limit_coefs(int32_t *k1, int32_t *k2)
+{
+	*k1 = shovel_k1;
+	*k2 = shovel_k2;
+	return shovel_current_limit;
+}
+
+void shovel_current_limit_enable(uint8_t enable)
+{
+	shovel_current_limit = enable;
+}
+
+/* Set CS command for shovel. Called by CS manager. */
+void shovel_set(void *mot, int32_t cmd)
+{
+	static int32_t oldpos;
+	int32_t pos, maxcmd, speed;
+
+	pos = encoders_spi_get_value(SHOVEL_ENCODER);
+	if (shovel_current_limit) {
+		speed = pos - oldpos;
+		if (speed > 0 && cmd < 0)
+			maxcmd = shovel_k1;
+		else if (speed < 0 && cmd > 0)
+			maxcmd = shovel_k1;
+		else {
+			speed = ABS(speed);
+			maxcmd = shovel_k1 + shovel_k2 * speed;
+		}
+		if (cmd > maxcmd)
+			cmd = maxcmd;
+		else if (cmd < -maxcmd)
+			cmd = -maxcmd;
+	}
+
+	pwm_ng_set(mot, cmd);
+	oldpos = pos;
+}
+
 void shovel_down(void)
 {
 	quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500);
diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h
index 80bceec..fa0e2a2 100644
--- a/projects/microb2010/cobboard/shovel.h
+++ b/projects/microb2010/cobboard/shovel.h
@@ -24,6 +24,11 @@
 
 void shovel_init(void);
 
+void shovel_set_current_limit_coefs(int32_t k1, int32_t k2);
+uint8_t shovel_get_current_limit_coefs(int32_t *k1, int32_t *k2);
+void shovel_current_limit_enable(uint8_t enable);
+void shovel_set(void *mot, int32_t cmd);
+
 void shovel_down(void);
 void shovel_mid(void);
 void shovel_up(void);
diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index f89580c..297dbce 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -11,7 +11,7 @@ ROBOT_HEIGHT=5 # 350
 CORN_HEIGHT=5  # 150
 
 ROBOT_WIDTH=320
-ROBOT_LENGTH=360
+ROBOT_LENGTH=250
 
 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)
@@ -63,8 +63,10 @@ sq2 = square(500)
 robot_x = 0.
 robot_y = 0.
 robot_a = 0.
-robot_lspickle = 0
-robot_rspickle = 0
+robot_lspickle_deployed = 0
+robot_rspickle_deployed = 0
+robot_lspickle_autoharvest = 0
+robot_rspickle_autoharvest = 0
 robot_trail = curve()
 robot_trail_list = []
 max_trail = 500
@@ -281,19 +283,25 @@ def set_robot():
     robot.axis = axis
     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),
+    lspickle.pos = (tmp_x + (robot_lspickle_deployed*60) * math.cos((tmp_a+90)*math.pi/180),
+                    tmp_y + (robot_lspickle_deployed*60) * math.sin((tmp_a+90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     lspickle.axis = axis
     lspickle.size = (20, ROBOT_WIDTH, 5)
+    if robot_lspickle_autoharvest:
+        lspickle.color = (1, 0, 0)
+    else:
+        lspickle.color = (0.4, 0.4, 0.4)
 
-    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),
+    rspickle.pos = (tmp_x + (robot_rspickle_deployed*60) * math.cos((tmp_a-90)*math.pi/180),
+                    tmp_y + (robot_rspickle_deployed*60) * math.sin((tmp_a-90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     rspickle.axis = axis
     rspickle.size = (20, ROBOT_WIDTH, 5)
+    if robot_rspickle_autoharvest:
+        rspickle.color = (1, 0, 0)
+    else:
+        rspickle.color = (0.4, 0.4, 0.4)
 
     # save position
     save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
@@ -360,15 +368,11 @@ while True:
                     side = int(m.groups()[0])
                     flags = int(m.groups()[1])
                     if side == 0:
-                        if (flags & 0x01) == 0:
-                            robot_lspickle = 1
-                        else:
-                            robot_lspickle = 2
+                        robot_lspickle_deployed = ((flags & 1) * 2)
+                        robot_lspickle_autoharvest = ((flags & 2) != 0)
                     else:
-                        if (flags & 0x01) == 0:
-                            robot_rspickle = 1
-                        else:
-                            robot_rspickle = 2
+                        robot_rspickle_deployed = ((flags & 1) * 2)
+                        robot_rspickle_autoharvest = ((flags & 2) != 0)
 
             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 0798699..00b6e86 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.9
+#define EXT_TRACK_MM 304.61875
 #define VIRTUAL_TRACK_MM EXT_TRACK_MM
 
-#define ROBOT_HALF_LENGTH_FRONT 180
-#define ROBOT_HALF_LENGTH_REAR 70
+#define ROBOT_HALF_LENGTH_FRONT 130
+#define ROBOT_HALF_LENGTH_REAR 120
 #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..42a33d9 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();
diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h
index 0950af0..7c1eccf 100644
--- a/projects/microb2010/mainboard/robotsim.h
+++ b/projects/microb2010/mainboard/robotsim.h
@@ -25,4 +25,5 @@ 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);