X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fspickle.c;h=f8e16130a16ebacfa265bf4f641bafdb76db71f8;hp=03565ac63a2f35021dbfacce82eee901eb66f03d;hb=4e7801883ed4076cb14b63a0571467747894c0f8;hpb=57b3e145f2ff46e1a7e7d522c85f3c56821dbf48

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index 03565ac..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
@@ -54,47 +54,44 @@ struct spickle_params {
 	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,
+	.k1 = 1000,
 	.k2 = 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 */
+		7000, // 200, /* left */
+		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. */
@@ -102,7 +99,7 @@ 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;
@@ -140,33 +137,26 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2)
 	spickle.k2 = k2;
 }
 
-void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed)
+
+void spickle_set_pos(uint8_t side, int32_t pos_packed,
+		     int32_t pos_mid, int32_t pos_deployed)
 {
 	spickle.pos_deployed[side] = pos_deployed;
+	spickle.pos_mid[side] = pos_mid;
 	spickle.pos_packed[side] = pos_packed;
 }
 
-void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed)
-{
-	spickle.delay_deployed[side] = delay_deployed;
-	spickle.delay_packed[side] = delay_packed;
-}
-
 void spickle_dump_params(void)
 {
 	printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
 	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)
@@ -174,24 +164,19 @@ void spickle_deploy(uint8_t side)
 	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
 }
 
-void spickle_pack(uint8_t side)
+void spickle_mid(uint8_t side)
 {
-	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
+	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
 }
 
-uint16_t spickle_get_deploy_delay(uint8_t side)
-{
-	return spickle.delay_deployed[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;
 }