X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fspickle.c;h=f8e16130a16ebacfa265bf4f641bafdb76db71f8;hp=db954ed46cf0e48dad40136bbb4819f734e5a804;hb=b28db6087ed5fdab0bda8f23417ef7098d2a9ae5;hpb=1714f4ee916fca95ce24120ea6e698237913f947 diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index db954ed..f8e1613 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -42,39 +42,64 @@ #include "sensor.h" #include "../common/i2c_commands.h" -#include "actuator.h" #include "main.h" +#include "actuator.h" - -#define OFF 0 -#define WAIT_SENSOR 1 -#define SENSOR_OK 2 -#define WAIT_DOWN 3 - -static volatile uint8_t spickle_state = OFF; -static volatile uint32_t spickle_pos_up = 35000; -static volatile uint32_t spickle_pos_down = 0; -static volatile uint32_t spickle_delay_up = 500; -static volatile uint32_t spickle_delay_down = 2000; -static volatile uint32_t delay = 0; -static volatile int32_t spickle_k1 = 500, spickle_k2 = 20; -static volatile int32_t spickle_cmd = 0; +struct spickle_params { + /* current limit (common to left and right) */ + int32_t k1; + int32_t k2; + + /* cs blocks */ + struct cs_block * const csb[2]; + + /* params */ + int32_t pos_deployed[2]; + int32_t pos_mid[2]; + int32_t pos_packed[2]; +}; + +static struct spickle_params spickle = { + .k1 = 1000, + .k2 = 20, + .csb = { + &cobboard.left_spickle, + &cobboard.right_spickle, + }, + .pos_deployed = { + 7000, // 200, /* left */ + 7000, // 200, /* right */ + }, + .pos_mid = { + 25000, /* left */ + 26000, /* right */ + }, + .pos_packed = { + 55800, /* left */ + 55800, /* right */ + }, +}; /* init spickle position at beginning */ static void spickle_autopos(void) { - pwm_ng_set(LEFT_SPICKLE_PWM, -500); - wait_ms(3000); + 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 */ +/* Set CS command for spickle. Called by CS manager. */ void spickle_set(void *mot, int32_t cmd) { static int32_t oldpos_left, oldpos_right; int32_t oldpos, pos, maxcmd, speed; - + if (mot == LEFT_SPICKLE_PWM) { pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER); oldpos = oldpos_left; @@ -86,12 +111,12 @@ void spickle_set(void *mot, int32_t cmd) speed = pos - oldpos; if (speed > 0 && cmd < 0) - maxcmd = spickle_k1; + maxcmd = spickle.k1; else if (speed < 0 && cmd > 0) - maxcmd = spickle_k1; + maxcmd = spickle.k1; else { speed = ABS(speed); - maxcmd = spickle_k1 + spickle_k2 * speed; + maxcmd = spickle.k1 + spickle.k2 * speed; } if (cmd > maxcmd) cmd = maxcmd; @@ -108,93 +133,50 @@ void spickle_set(void *mot, int32_t cmd) void spickle_set_coefs(uint32_t k1, uint32_t k2) { - spickle_k1 = k1; - spickle_k2 = k2; + spickle.k1 = k1; + spickle.k2 = k2; } -void spickle_set_delays(uint32_t delay_up, uint32_t delay_down) -{ - spickle_delay_up = delay_up; - spickle_delay_down = delay_down; -} -void spickle_set_pos(uint32_t pos_up, uint32_t pos_down) +void spickle_set_pos(uint8_t side, int32_t pos_packed, + int32_t pos_mid, int32_t pos_deployed) { - spickle_pos_up = pos_up; - spickle_pos_down = pos_down; + 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("pos %ld %ld\r\n"), spickle_pos_up, spickle_pos_down); - printf_P(PSTR("delay %ld %ld\r\n"), spickle_delay_up, spickle_delay_down); + printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2); + printf_P(PSTR("left pos %ld %ld\r\n"), + 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_packed[I2C_RIGHT_SIDE], + spickle.pos_mid[I2C_RIGHT_SIDE], + spickle.pos_deployed[I2C_RIGHT_SIDE]); } -void spickle_up(void) +void spickle_deploy(uint8_t side) { - spickle_state = 0; - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); } -void spickle_down(void) +void spickle_mid(uint8_t side) { - spickle_state = 0; - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down); + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]); } -void spickle_stop(void) +void spickle_pack(uint8_t side) { - spickle_state = 0; -} - -void spickle_auto(void) -{ - spickle_state = WAIT_SENSOR; - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); -} - -/* for spickle auto mode */ -static void spickle_cb(__attribute__((unused)) void *dummy) -{ - static uint8_t prev = 0; - uint8_t val; - - val = sensor_get(S_LCOB); - - switch (spickle_state) { - case OFF: - break; - case WAIT_SENSOR: - if (val && !prev) { - delay = spickle_delay_up; - spickle_state = SENSOR_OK; - } - break; - case SENSOR_OK: - if (delay-- == 0) { - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down); - spickle_state = WAIT_DOWN; - delay = spickle_delay_down; - } - break; - case WAIT_DOWN: - if (delay-- == 0) { - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); - spickle_state = WAIT_SENSOR; - } - break; - default: - break; - } - prev = val; + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]); } void spickle_init(void) { spickle_autopos(); - - scheduler_add_periodical_event_priority(spickle_cb, NULL, - 1000L / SCHEDULER_UNIT, - SPICKLE_PRIO); + cobboard.left_spickle.on = 1; + cobboard.right_spickle.on = 1; }