X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fspickle.c;h=741c103155e48826dbb383c573ca6b2f363b19e7;hp=62732c208cdfa24589dcba374bb5144396a3ae1c;hb=d23a4fdeed4e90d004d407942b230c188d012465;hpb=0c4732c1e255ea56aa766b772dfdb6be49620830 diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 62732c2..741c103 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -42,59 +42,61 @@ #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]; /* params */ - int16_t delay_deployed[2]; - int16_t delay_packed[2]; int32_t pos_deployed[2]; + int32_t pos_mid[2]; int32_t pos_packed[2]; }; static struct spickle_params spickle = { - .k1 = 500, - .k2 = 20, + .sk1 = 1000, + .sk2 = 20, + .wk1 = 200, + .wk2 = 20, .csb = { &cobboard.left_spickle, &cobboard.right_spickle, }, - .delay_deployed = { - 500, /* left */ - 500, /* right */ - }, - .delay_packed = { - 500, /* left */ - 500, /* right */ - }, .pos_deployed = { - 35000, /* left */ - 35000, /* right */ + 500,// 7000, // 200, /* left */ + 500,// 7000, // 200, /* right */ + }, + .pos_mid = { + 25000, /* left */ + 26000, /* right */ }, .pos_packed = { - 0, /* left */ - 0, /* right */ + 55800, /* left */ + 55800, /* right */ }, }; /* init spickle position at beginning */ static void spickle_autopos(void) { - pwm_ng_set(LEFT_SPICKLE_PWM, -500); - //pwm_ng_set(RIGHT_SPICKLE_PWM, -500); - wait_ms(1000); + printf_P(PSTR("spickle autopos...")); + 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); encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0); + printf_P(PSTR("ok\r\n")); } /* Set CS command for spickle. Called by CS manager. */ @@ -102,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; @@ -134,64 +153,84 @@ 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_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed) +void spickle_set_scoefs(uint32_t k1, uint32_t k2) { - spickle.pos_deployed[side] = pos_deployed; - spickle.pos_packed[side] = pos_packed; + spickle.sk1 = k1; + spickle.sk2 = k2; } -void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed) +void spickle_set_pos(uint8_t side, int32_t pos_packed, + int32_t pos_mid, int32_t pos_deployed) { - spickle.delay_deployed[side] = delay_deployed; - spickle.delay_packed[side] = delay_packed; + spickle.pos_deployed[side] = pos_deployed; + spickle.pos_mid[side] = pos_mid; + spickle.pos_packed[side] = 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_deployed[I2C_LEFT_SIDE], - spickle.pos_packed[I2C_LEFT_SIDE]); - printf_P(PSTR("left delay %ld %ld\r\n"), - spickle.delay_deployed[I2C_LEFT_SIDE], - spickle.delay_packed[I2C_LEFT_SIDE]); + spickle.pos_packed[I2C_LEFT_SIDE], + spickle.pos_mid[I2C_LEFT_SIDE], + spickle.pos_deployed[I2C_LEFT_SIDE]); printf_P(PSTR("right pos %ld %ld\r\n"), - spickle.pos_deployed[I2C_RIGHT_SIDE], - spickle.pos_packed[I2C_RIGHT_SIDE]); - printf_P(PSTR("right delay %ld %ld\r\n"), - spickle.delay_deployed[I2C_RIGHT_SIDE], - spickle.delay_packed[I2C_RIGHT_SIDE]); + spickle.pos_packed[I2C_RIGHT_SIDE], + spickle.pos_mid[I2C_RIGHT_SIDE], + spickle.pos_deployed[I2C_RIGHT_SIDE]); } -void spickle_deploy(uint8_t side) +static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos) { - cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); + 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; } -void spickle_pack(uint8_t side) +uint8_t spickle_is_packed(uint8_t side) { - cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[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]); } -uint16_t spickle_get_deploy_delay(uint8_t side) +void spickle_deploy(uint8_t side) { - return spickle.delay_deployed[side]; + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); } -uint16_t spickle_get_pack_delay(uint8_t side) +void spickle_mid(uint8_t side) { - return spickle.delay_packed[side]; + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]); +} + +void spickle_pack(uint8_t side) +{ + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]); } void spickle_init(void) { spickle_autopos(); cobboard.left_spickle.on = 1; - //cobboard.right_spickle.on = 1; + cobboard.right_spickle.on = 1; }