weak current limit on spickles

[aversive.git] / projects / microb2010 / cobboard / spickle.c

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index e1d9a7b..741c103 100644 (file)
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -42,13 +42,16 @@
 
 #include "sensor.h"
 #include "../common/i2c_commands.h"
+#include "state.h"
 #include "main.h"
 #include "actuator.h"
 
 struct spickle_params {
        /* current limit (common to left and right) */
-       int32_t k1;
-       int32_t k2;
+       int32_t sk1;
+       int32_t sk2;
+       int32_t wk1;
+       int32_t wk2;
 
        /* cs blocks */
        struct cs_block * const csb[2];
@@ -60,23 +63,25 @@ struct spickle_params {
 };
 
 static struct spickle_params spickle = {
-       .k1 = 500,
-       .k2 = 20,
+       .sk1 = 1000,
+       .sk2 = 20,
+       .wk1 = 200,
+       .wk2 = 20,
        .csb = {
                &cobboard.left_spickle,
                &cobboard.right_spickle,
        },
        .pos_deployed = {
-               40000, /* left */
-               -40000, /* right */
+               500,// 7000, // 200, /* left */
+               500,// 7000, // 200, /* right */
        },
        .pos_mid = {
-               20000, /* left */
-               -20000, /* right */
+               25000, /* left */
+               26000, /* right */
        },
        .pos_packed = {
-               0, /* left */
-               0, /* right */
+               55800, /* left */
+               55800, /* right */
        },
 };
 
@@ -84,9 +89,9 @@ static struct spickle_params spickle = {
 static void spickle_autopos(void)
 {
        printf_P(PSTR("spickle autopos..."));
-       pwm_ng_set(LEFT_SPICKLE_PWM, -500);
-       pwm_ng_set(RIGHT_SPICKLE_PWM, 500);
-       wait_ms(1000);
+       pwm_ng_set(LEFT_SPICKLE_PWM, -700);
+       pwm_ng_set(RIGHT_SPICKLE_PWM, -700);
+       wait_ms(2500);
        pwm_ng_set(LEFT_SPICKLE_PWM, 0);
        pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
        encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
@@ -99,24 +104,41 @@ void spickle_set(void *mot, int32_t cmd)
 {
        static int32_t oldpos_left, oldpos_right;
        int32_t oldpos, pos, maxcmd, speed;
+       int32_t k1, k2;
 
        if (mot == LEFT_SPICKLE_PWM) {
                pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
                oldpos = oldpos_left;
+               if (state_spicklemode_weak(I2C_LEFT_SIDE)) {
+                       k1 = spickle.wk1;
+                       k2 = spickle.wk2;
+               }
+               else {
+                       k1 = spickle.sk1;
+                       k2 = spickle.sk2;
+               }
        }
        else {
                pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
                oldpos = oldpos_right;
+               if (state_spicklemode_weak(I2C_RIGHT_SIDE)) {
+                       k1 = spickle.wk1;
+                       k2 = spickle.wk2;
+               }
+               else {
+                       k1 = spickle.sk1;
+                       k2 = spickle.sk2;
+               }
        }
 
        speed = pos - oldpos;
        if (speed > 0 && cmd < 0)
-               maxcmd = spickle.k1;
+               maxcmd = k1;
        else if (speed < 0 && cmd > 0)
-               maxcmd = spickle.k1;
+               maxcmd = k1;
        else {
                speed = ABS(speed);
-               maxcmd = spickle.k1 + spickle.k2 * speed;
+               maxcmd = k1 + k2 * speed;
        }
        if (cmd > maxcmd)
                cmd = maxcmd;
@@ -131,12 +153,17 @@ void spickle_set(void *mot, int32_t cmd)
                oldpos_right = pos;
 }
 
-void spickle_set_coefs(uint32_t k1, uint32_t k2)
+void spickle_set_wcoefs(uint32_t k1, uint32_t k2)
 {
-       spickle.k1 = k1;
-       spickle.k2 = k2;
+       spickle.wk1 = k1;
+       spickle.wk2 = k2;
 }
 
+void spickle_set_scoefs(uint32_t k1, uint32_t k2)
+{
+       spickle.sk1 = k1;
+       spickle.sk2 = k2;
+}
 
 void spickle_set_pos(uint8_t side, int32_t pos_packed,
                     int32_t pos_mid, int32_t pos_deployed)
@@ -148,7 +175,8 @@ void spickle_set_pos(uint8_t side, int32_t pos_packed,
 
 void spickle_dump_params(void)
 {
-       printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
+       printf_P(PSTR("strong_coef %ld %ld\r\n"), spickle.sk1, spickle.sk2);
+       printf_P(PSTR("weak_coef %ld %ld\r\n"), spickle.wk1, spickle.wk2);
        printf_P(PSTR("left pos %ld %ld\r\n"),
                 spickle.pos_packed[I2C_LEFT_SIDE],
                 spickle.pos_mid[I2C_LEFT_SIDE],
@@ -159,6 +187,32 @@ void spickle_dump_params(void)
                 spickle.pos_deployed[I2C_RIGHT_SIDE]);
 }
 
+static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
+{
+       int32_t diff;
+       int32_t enc;
+       if (side == I2C_LEFT_SIDE)
+               enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
+       else
+               enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
+       diff = pos - enc;
+       if (diff < 0)
+               diff = -diff;
+       if (diff < 500)
+               return 1;
+       return 0;
+}
+
+uint8_t spickle_is_packed(uint8_t side)
+{
+       return spickle_is_at_pos(side, spickle.pos_packed[side]);
+}
+
+uint8_t spickle_is_deployed(uint8_t side)
+{
+       return spickle_is_at_pos(side, spickle.pos_deployed[side]);
+}
+
 void spickle_deploy(uint8_t side)
 {
        cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);