X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fspickle.c;h=7296d8e28afec304687f90e0a37eac19c00944b5;hp=e1d9a7b3b7e8ea33703c39f5134096bbd489780f;hb=aa68be2d5d596693bda1109e4ccd6489f613b4f7;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index e1d9a7b..7296d8e 100644
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -60,23 +60,23 @@ struct spickle_params {
 };
 
 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 */
 	},
 };
 
@@ -84,9 +84,9 @@ static struct spickle_params spickle = {
 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);
@@ -137,7 +137,6 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2)
 	spickle.k2 = k2;
 }
 
-
 void spickle_set_pos(uint8_t side, int32_t pos_packed,
 		     int32_t pos_mid, int32_t pos_deployed)
 {
@@ -159,6 +158,32 @@ void spickle_dump_params(void)
 		 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]);