&cobboard.right_spickle,
},
.pos_deployed = {
- 200, /* left */
- 200, /* right */
+ 500,// 7000, // 200, /* left */
+ 500,// 7000, // 200, /* right */
},
.pos_mid = {
25000, /* left */
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]);