};
static struct spickle_params spickle = {
- .k1 = 500,
+ .k1 = 1000,
.k2 = 20,
.csb = {
&cobboard.left_spickle,
&cobboard.right_spickle,
},
.pos_deployed = {
- 40000, /* left */
- -40000, /* right */
+ 7000, // 200, /* left */
+ 7000, // 200, /* right */
},
.pos_mid = {
- 20000, /* left */
- -20000, /* right */
+ 25000, /* left */
+ 26000, /* right */
},
.pos_packed = {
- 0, /* left */
- 0, /* right */
+ 55800, /* left */
+ 55800, /* right */
},
};
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);
spickle.k2 = k2;
}
-
void spickle_set_pos(uint8_t side, int32_t pos_packed,
int32_t pos_mid, int32_t pos_deployed)
{
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]);