-/*
+/*
* 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
#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. */
{
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;
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]);
+}
+
+void spickle_deploy(uint8_t side)
+{
+ cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
}
-uint16_t spickle_get_deploy_delay(uint8_t side)
+void spickle_mid(uint8_t side)
{
- return spickle.delay_deployed[side];
+ cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
}
-uint16_t spickle_get_pack_delay(uint8_t side)
+void spickle_pack(uint8_t side)
{
- return spickle.delay_packed[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;
}
git.droids-corp.org / aversive.git / blobdiff