limit speed in weak pack

diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index 386b36024b07d108c2480e044b53cb6546eccaa4..31fa5e7e70b3ddcb20320bb69e68ad317b9d3597 100644 (file)
--- 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 de538867334a07369d2bfe4e42ddc4834ed30dd8..058a7b71ce74fe125e8202f656f6005dac84c352 100644 (file)
--- 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 04e8c9171120bde625ac2b2895ac3708e45a18fc..713fedf5d9ddf7784aa5e5f6fb7c4721886e6aa1 100644 (file)
--- 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);