From ad6e9ce0b9c0186038fefec49f80a15d9b76e178 Mon Sep 17 00:00:00 2001
From: zer0 <zer0@carbon.local>
Date: Sat, 8 May 2010 15:40:35 +0200
Subject: [PATCH] limit speed in weak pack

---
 projects/microb2010/cobboard/cs.c              | 12 ++++++++++++
 projects/microb2010/cobboard/spickle.c         | 18 ++++++++++++++++++
 .../microb2010/mainboard/commands_mainboard.c  |  4 ++--
 3 files changed, 32 insertions(+), 2 deletions(-)

diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index 386b360..31fa5e7 100644
--- a/projects/microb2010/cobboard/cs.c
+++ b/projects/microb2010/cobboard/cs.c
@@ -125,8 +125,14 @@ void microb_cs_init(void)
 	pid_set_out_shift(&cobboard.left_spickle.pid, 10);
 	pid_set_derivate_filter(&cobboard.left_spickle.pid, 4);
 
+	/* quadramp */
+	quadramp_init(&cobboard.left_spickle.qr);
+	quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 3000, 3000); /* set speed */
+	quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 0, 0); /* set accel */
+
 	/* CS */
 	cs_init(&cobboard.left_spickle.cs);
+	cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr);
 	cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid);
 	cs_set_process_in(&cobboard.left_spickle.cs, spickle_set, LEFT_SPICKLE_PWM);
 	cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER);
@@ -145,8 +151,14 @@ void microb_cs_init(void)
 	pid_set_out_shift(&cobboard.right_spickle.pid, 10);
 	pid_set_derivate_filter(&cobboard.right_spickle.pid, 4);
 
+	/* quadramp */
+	quadramp_init(&cobboard.right_spickle.qr);
+	quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 0, 0); /* set speed */
+	quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 0, 0); /* set accel */
+
 	/* CS */
 	cs_init(&cobboard.right_spickle.cs);
+	cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr);
 	cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid);
 	cs_set_process_in(&cobboard.right_spickle.cs, spickle_set, RIGHT_SPICKLE_PWM);
 	cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER);
diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index de53886..058a7b7 100644
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -203,6 +203,21 @@ static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
 	return 0;
 }
 
+static void spickle_set_qr(uint8_t side)
+{
+	struct quadramp_filter *qr;
+
+	if (side == I2C_LEFT_SIDE)
+		qr = &cobboard.left_spickle.qr;
+	else
+		qr = &cobboard.right_spickle.qr;
+
+	if (state_spicklemode_weak(side))
+		quadramp_set_1st_order_vars(qr, 700, 700); /* set speed */
+	else
+		quadramp_set_1st_order_vars(qr, 3000, 3000); /* set speed */
+}
+
 uint8_t spickle_is_packed(uint8_t side)
 {
 	return spickle_is_at_pos(side, spickle.pos_packed[side]);
@@ -215,16 +230,19 @@ uint8_t spickle_is_deployed(uint8_t side)
 
 void spickle_deploy(uint8_t side)
 {
+	spickle_set_qr(side);
 	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
 }
 
 void spickle_mid(uint8_t side)
 {
+	spickle_set_qr(side);
 	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
 }
 
 void spickle_pack(uint8_t side)
 {
+	spickle_set_qr(side);
 	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
 }
 
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index 04e8c91..713fedf 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -792,7 +792,7 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 		i2c_cobboard_pack(side);
 	}
-	else if (!strcmp_P(res->arg1, PSTR("pack_weak"))) {
+	else if (!strcmp_P(res->arg1, PSTR("weak_pack"))) {
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 		i2c_cobboard_pack_weak(side);
 	}
@@ -808,7 +808,7 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
 
 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#pack_weak#harvest_nomove#deploy_nomove";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
 prog_char str_cobboard_setmode2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
-- 
2.39.5