cobboard

diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c
index cebcd2a..79f91cd 100644 (file)
--- a/projects/microb2010/cobboard/actuator.c
+++ b/projects/microb2010/cobboard/actuator.c
@@ -48,19 +48,27 @@
 #define COBROLLER_SPEED 800
 //#define COBROLLER_SPEED 400
 
 #define COBROLLER_SPEED 800
 //#define COBROLLER_SPEED 400
 

-#define SERVO_DOOR_OPEN 250
-#define SERVO_DOOR_CLOSED 470
+#define SERVO_DOOR_OPEN 260
+#define SERVO_DOOR_CLOSED 490
+
+#define SERVO_CARRY_L_OPEN 280
+#define SERVO_CARRY_L_CLOSED 510
+
+#define SERVO_CARRY_R_OPEN 470
+#define SERVO_CARRY_R_CLOSED 250

 
 void actuator_init(void);
 
 void servo_carry_open(void)
 {
 
 void actuator_init(void);
 
 void servo_carry_open(void)
 {

-       /* TODO */
+       pwm_ng_set(SERVO_CARRY_L_PWM, SERVO_CARRY_L_OPEN);
+       pwm_ng_set(SERVO_CARRY_R_PWM, SERVO_CARRY_R_OPEN);

 }
 
 void servo_carry_close(void)
 {
 }
 
 void servo_carry_close(void)
 {

-       /* TODO */
+       pwm_ng_set(SERVO_CARRY_L_PWM, SERVO_CARRY_L_CLOSED);
+       pwm_ng_set(SERVO_CARRY_R_PWM, SERVO_CARRY_R_CLOSED);

 }
 
 void servo_door_open(void)
 }
 
 void servo_door_open(void)

diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index 17d8e54..f9c0b7d 100644 (file)
--- a/projects/microb2010/cobboard/cs.c
+++ b/projects/microb2010/cobboard/cs.c
@@ -47,13 +47,20 @@
 #include "actuator.h"
 #include "spickle.h"
 
 #include "actuator.h"
 #include "spickle.h"
 

+#define DEBUG_CPLD
+

 /* called every 5 ms */
 /* called every 5 ms */

-static void do_cs(__attribute__((unused)) void *dummy) 
+static void do_cs(__attribute__((unused)) void *dummy)

 {
        /* read encoders */
        if (cobboard.flags & DO_ENCODERS) {
                encoders_spi_manage(NULL);
        }
 {
        /* read encoders */
        if (cobboard.flags & DO_ENCODERS) {
                encoders_spi_manage(NULL);
        }

+#ifdef DEBUG_CPLD
+       cobboard.left_spickle.prev = cobboard.left_spickle.cs.filtered_feedback_value;
+       cobboard.right_spickle.prev = cobboard.right_spickle.cs.filtered_feedback_value;
+       cobboard.shovel.prev = cobboard.shovel.cs.filtered_feedback_value;
+#endif

        /* control system */
        if (cobboard.flags & DO_CS) {
                if (cobboard.left_spickle.on)
        /* control system */
        if (cobboard.flags & DO_CS) {
                if (cobboard.left_spickle.on)


@@ -63,6 +70,31 @@ static void do_cs(__attribute__((unused)) void *dummy)
                if (cobboard.shovel.on)
                        cs_manage(&cobboard.shovel.cs);
        }
                if (cobboard.shovel.on)
                        cs_manage(&cobboard.shovel.cs);
        }

+#ifdef DEBUG_CPLD
+       {
+               extern int16_t g_encoders_spi_previous[4];
+               int32_t ls, rs, sh;
+
+               ls = (cobboard.left_spickle.prev - cobboard.left_spickle.cs.filtered_feedback_value);
+               rs = (cobboard.right_spickle.prev - cobboard.right_spickle.cs.filtered_feedback_value);
+               sh = (cobboard.shovel.prev - cobboard.shovel.cs.filtered_feedback_value);
+               if (ls < -2000 || ls > 2000 ||
+                   rs < -2000 || rs > 2000 ||
+                   sh < -2000 || sh > 2000) {
+                       printf_P(PSTR("left_spickle %ld "), ls);
+                       printf_P(PSTR("right_spickle %ld "), rs);
+                       printf_P(PSTR("shovel %ld "), sh);
+                       printf_P(PSTR("/ %d %d %d %d\r\n"),
+                                g_encoders_spi_previous[0],
+                                g_encoders_spi_previous[1],
+                                g_encoders_spi_previous[2],
+                                g_encoders_spi_previous[3]);
+                       BRAKE_ON();
+                       while (1);
+               }
+       }
+#endif
+

        if (cobboard.flags & DO_BD) {
                bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs);
                bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs);
        if (cobboard.flags & DO_BD) {
                bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs);
                bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs);


@@ -77,7 +109,7 @@ static void do_cs(__attribute__((unused)) void *dummy)
 void dump_cs_debug(const char *name, struct cs *cs)
 {
        DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
 void dump_cs_debug(const char *name, struct cs *cs)
 {
        DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "

-             "in=% .5ld out=% .5ld", 
+             "in=% .5ld out=% .5ld",

              name, cs_get_consign(cs), cs_get_filtered_consign(cs),
              cs_get_error(cs), cs_get_filtered_feedback(cs),
              cs_get_out(cs));
              name, cs_get_consign(cs), cs_get_filtered_consign(cs),
              cs_get_error(cs), cs_get_filtered_feedback(cs),
              cs_get_out(cs));


@@ -86,7 +118,7 @@ void dump_cs_debug(const char *name, struct cs *cs)
 void dump_cs(const char *name, struct cs *cs)
 {
        printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
 void dump_cs(const char *name, struct cs *cs)
 {
        printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "

-                     "in=% .5ld out=% .5ld\r\n"), 
+                     "in=% .5ld out=% .5ld\r\n"),

                 name, cs_get_consign(cs), cs_get_filtered_consign(cs),
                 cs_get_error(cs), cs_get_filtered_feedback(cs),
                 cs_get_out(cs));
                 name, cs_get_consign(cs), cs_get_filtered_consign(cs),
                 cs_get_error(cs), cs_get_filtered_feedback(cs),
                 cs_get_out(cs));


@@ -147,14 +179,19 @@ void microb_cs_init(void)
        /* ---- CS shovel */
        /* PID */
        pid_init(&cobboard.shovel.pid);
        /* ---- CS shovel */
        /* PID */
        pid_init(&cobboard.shovel.pid);

-       pid_set_gains(&cobboard.shovel.pid, 1000, 10, 200);
+       pid_set_gains(&cobboard.shovel.pid, 1000, 10, 1400);

        pid_set_maximums(&cobboard.shovel.pid, 0, 10000, 3200); /* max is 18 V */
        pid_set_out_shift(&cobboard.shovel.pid, 10);
        pid_set_derivate_filter(&cobboard.shovel.pid, 4);
 
        pid_set_maximums(&cobboard.shovel.pid, 0, 10000, 3200); /* max is 18 V */
        pid_set_out_shift(&cobboard.shovel.pid, 10);
        pid_set_derivate_filter(&cobboard.shovel.pid, 4);
 

+       /* quadramp */
+       quadramp_init(&cobboard.shovel.qr);
+       quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2000); /* set speed */
+       quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 20); /* set accel */
+

        /* CS */
        cs_init(&cobboard.shovel.cs);
        /* CS */
        cs_init(&cobboard.shovel.cs);

-       //cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
+       cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);

        cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
        cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
        cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);
        cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
        cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
        cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);


@@ -170,7 +207,7 @@ void microb_cs_init(void)
        cobboard.right_spickle.on = 0;
        cobboard.shovel.on = 0;
 
        cobboard.right_spickle.on = 0;
        cobboard.shovel.on = 0;
 

-       scheduler_add_periodical_event_priority(do_cs, NULL, 
-                                               CS_PERIOD / SCHEDULER_UNIT, 
+       scheduler_add_periodical_event_priority(do_cs, NULL,
+                                               CS_PERIOD / SCHEDULER_UNIT,

                                                CS_PRIO);
 }
                                                CS_PRIO);
 }

diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c
index 39318d9..62103c8 100755 (executable)
--- a/projects/microb2010/cobboard/main.c
+++ b/projects/microb2010/cobboard/main.c
@@ -1,7 +1,7 @@
-/*  
+/*

  *  Copyright Droids Corporation
  *  Olivier Matz <zer0@droids-corp.org>
  *  Copyright Droids Corporation
  *  Olivier Matz <zer0@droids-corp.org>

- * 
+ *

  *  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
  *  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


@@ -65,7 +65,7 @@
 /* 0 means "programmed"
  * ---- with 16 Mhz quartz
  * CKSEL 3-0 : 0111
 /* 0 means "programmed"
  * ---- with 16 Mhz quartz
  * CKSEL 3-0 : 0111

- * SUT 1-0 : 10 
+ * SUT 1-0 : 10

  * CKDIV8 : 1
  * ---- bootloader
  * BOOTZ 1-0 : 01 (4K bootloader)
  * CKDIV8 : 1
  * ---- bootloader
  * BOOTZ 1-0 : 01 (4K bootloader)


@@ -109,7 +109,7 @@ void bootloader(void)
        __asm__ __volatile__ ("ldi r31,0xf8\n");
        __asm__ __volatile__ ("ldi r30,0x00\n");
        __asm__ __volatile__ ("eijmp\n");
        __asm__ __volatile__ ("ldi r31,0xf8\n");
        __asm__ __volatile__ ("ldi r30,0x00\n");
        __asm__ __volatile__ ("eijmp\n");

-       
+

        /* never returns */
 }
 
        /* never returns */
 }
 


@@ -122,7 +122,7 @@ void do_led_blink(__attribute__((unused)) void *dummy)
                LED1_ON();
        else
                LED1_OFF();
                LED1_ON();
        else
                LED1_OFF();

-       
+

        a = !a;
 #endif
 }
        a = !a;
 #endif
 }


@@ -183,6 +183,8 @@ int main(void)
        error_register_notice(mylog);
        error_register_debug(mylog);
 
        error_register_notice(mylog);
        error_register_debug(mylog);
 

+       wait_ms(3000);
+

        /* SPI + ENCODERS */
        encoders_spi_init(); /* this will also init spi hardware */
 
        /* SPI + ENCODERS */
        encoders_spi_init(); /* this will also init spi hardware */
 


@@ -196,11 +198,11 @@ int main(void)
        timer0_register_OV_intr(main_timer_interrupt);
 
        /* PWM */
        timer0_register_OV_intr(main_timer_interrupt);
 
        /* PWM */

-       PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, 
+       PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,

                                 TIMER1_PRESCALER_DIV_1);
                                 TIMER1_PRESCALER_DIV_1);

-       PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, 
+       PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,

                                 TIMER4_PRESCALER_DIV_1);
                                 TIMER4_PRESCALER_DIV_1);

-       
+

        PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
                      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
        PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
        PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
                      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
        PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,


@@ -213,11 +215,11 @@ int main(void)
 
 
        /* servos */
 
 
        /* servos */

-       PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, 
+       PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,

                                 TIMER1_PRESCALER_DIV_256);
        PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);
                                 TIMER1_PRESCALER_DIV_256);
        PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);

-       PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10, 
+       PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10,

                                 TIMER1_PRESCALER_DIV_256);
        PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);
                                 TIMER1_PRESCALER_DIV_256);
        PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);


@@ -225,12 +227,12 @@ int main(void)
                      NULL, 0);
        PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);
                      NULL, 0);
        PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);

-       
+

        /* SCHEDULER */
        scheduler_init();
 
        /* SCHEDULER */
        scheduler_init();
 

-       scheduler_add_periodical_event_priority(do_led_blink, NULL, 
-                                               100000L / SCHEDULER_UNIT, 
+       scheduler_add_periodical_event_priority(do_led_blink, NULL,
+                                               100000L / SCHEDULER_UNIT,

                                                LED_PRIO);
        /* all cs management */
        microb_cs_init();
                                                LED_PRIO);
        /* all cs management */
        microb_cs_init();

diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h
index a0dbac8..0d07a9c 100755 (executable)
--- a/projects/microb2010/cobboard/main.h
+++ b/projects/microb2010/cobboard/main.h
@@ -1,6 +1,6 @@
-/*  
+/*

  *  Copyright Droids Corporation (2009)
  *  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
  *  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


@@ -51,6 +51,8 @@
 #define SHOVEL_ENCODER         ((void *)2)
 
 #define SERVO_DOOR_PWM         ((void *)&gen.servo2)
 #define SHOVEL_ENCODER         ((void *)2)
 
 #define SERVO_DOOR_PWM         ((void *)&gen.servo2)

+#define SERVO_CARRY_L_PWM      ((void *)&gen.servo1)
+#define SERVO_CARRY_R_PWM      ((void *)&gen.servo3)

 
 #define LEFT_SPICKLE_PWM       ((void *)&gen.pwm1_4A)
 #define RIGHT_SPICKLE_PWM      ((void *)&gen.pwm2_4B)
 
 #define LEFT_SPICKLE_PWM       ((void *)&gen.pwm1_4A)
 #define RIGHT_SPICKLE_PWM      ((void *)&gen.pwm2_4B)


@@ -92,7 +94,7 @@ struct genboard {
        struct pwm_ng servo2;
        struct pwm_ng servo3;
        struct pwm_ng servo4;
        struct pwm_ng servo2;
        struct pwm_ng servo3;
        struct pwm_ng servo4;

-       
+

        /* ax12 interface */
        AX12 ax12;
 
        /* ax12 interface */
        AX12 ax12;
 


@@ -104,6 +106,7 @@ struct genboard {
 
 struct cs_block {
        uint8_t on;
 
 struct cs_block {
        uint8_t on;

+       int32_t prev;

         struct cs cs;
         struct pid_filter pid;
        struct quadramp_filter qr;
         struct cs cs;
         struct pid_filter pid;
        struct quadramp_filter qr;

diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c
index eec58b1..b820d98 100644 (file)
--- a/projects/microb2010/cobboard/sensor.c
+++ b/projects/microb2010/cobboard/sensor.c
@@ -143,8 +143,8 @@ struct sensor_filter {
  */
 static struct sensor_filter sensor_filter[SENSOR_MAX] = {
        [S_CAP1] =      { 10, 0, 3, 7, 0, 0 }, /* 0 */
  */
 static struct sensor_filter sensor_filter[SENSOR_MAX] = {
        [S_CAP1] =      { 10, 0, 3, 7, 0, 0 }, /* 0 */

-       [S_FRONT] =     { 5, 0, 4, 1, 0, 0 },  /* 1 */
-       [S_CAP3] =      { 10, 0, 3, 7, 0, 0 }, /* 2 */
+       [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 0 }, /* 1 */
+       [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */

        [S_CAP4] =      { 1, 0, 0, 1, 0, 0 }, /* 3 */
        [S_LCOB] =      { 1, 0, 0, 1, 0, 0 }, /* 4 */
        [S_LEFT] =      { 5, 0, 4, 1, 0, 0 }, /* 5 */
        [S_CAP4] =      { 1, 0, 0, 1, 0, 0 }, /* 3 */
        [S_LCOB] =      { 1, 0, 0, 1, 0, 0 }, /* 4 */
        [S_LEFT] =      { 5, 0, 4, 1, 0, 0 }, /* 5 */

diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h
index 6f47742..4e634fc 100644 (file)
--- a/projects/microb2010/cobboard/sensor.h
+++ b/projects/microb2010/cobboard/sensor.h
@@ -29,8 +29,8 @@
 
 /* synchronize with sensor.c */
 #define S_CAP1          0
 
 /* synchronize with sensor.c */
 #define S_CAP1          0

-#define S_FRONT         1
-#define S_CAP3          2
+#define S_COB_INSIDE_L  1
+#define S_COB_INSIDE_R  2

 #define S_CAP4          3
 #define S_LCOB          4
 #define S_LEFT          5
 #define S_CAP4          3
 #define S_LCOB          4
 #define S_LEFT          5

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index 3686eb7..62732c2 100644 (file)
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -1,6 +1,6 @@
-/*  
+/*

  *  Copyright Droids Corporation (2009)
  *  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
  *  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


@@ -102,7 +102,7 @@ void spickle_set(void *mot, int32_t cmd)
 {
        static int32_t oldpos_left, oldpos_right;
        int32_t oldpos, pos, maxcmd, speed;
 {
        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;
        if (mot == LEFT_SPICKLE_PWM) {
                pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
                oldpos = oldpos_left;

diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index 2903a4a..3308405 100644 (file)
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -114,6 +114,13 @@ static uint8_t state_cob_color(uint8_t side)
                return 0;
 }
 
                return 0;
 }
 

+/* return true if the cob is correctly inside */
+static uint8_t state_cob_inside(void)
+{
+       return sensor_get(S_COB_INSIDE_L) &&
+               sensor_get(S_COB_INSIDE_R);
+}
+

 /* set a new state, return 0 on success */
 int8_t state_set_mode(uint8_t mode)
 {
 /* set a new state, return 0 on success */
 int8_t state_set_mode(uint8_t mode)
 {


@@ -163,23 +170,34 @@ static void state_do_harvest(uint8_t side)
 
        /* eat the cob */
        spickle_pack(side);
 
        /* eat the cob */
        spickle_pack(side);

-       state_debug_wait_key_pressed();

        /* xxx */
        time_wait_ms(250);
        left_cobroller_on();
        delay = spickle_get_pack_delay(side);
        /* xxx */
        time_wait_ms(250);
        left_cobroller_on();
        delay = spickle_get_pack_delay(side);

-       time_wait_ms(delay);
+
+       WAIT_COND_OR_TIMEOUT(state_cob_inside(), delay);

 
        /* redeploy the spickle */
        spickle_deploy(side);
        state_debug_wait_key_pressed();
 
 
        /* redeploy the spickle */
        spickle_deploy(side);
        state_debug_wait_key_pressed();
 

+       /* let the cob go */
+       servo_carry_open();
+       wait_ms(300); /* XXX */
+       state_debug_wait_key_pressed();
+

        cob_count ++;
 
        /* store it */
        shovel_up();
        wait_ms(200);
        state_debug_wait_key_pressed();
        cob_count ++;
 
        /* store it */
        shovel_up();
        wait_ms(200);
        state_debug_wait_key_pressed();

+
+       /* close the carry servos */
+       servo_carry_close();
+       wait_ms(300); /* XXX */
+       state_debug_wait_key_pressed();
+

        shovel_down();
        left_cobroller_off();
        state_debug_wait_key_pressed();
        shovel_down();
        left_cobroller_off();
        state_debug_wait_key_pressed();


@@ -191,10 +209,12 @@ static void state_do_eject(void)
 {
        cob_count = 0;
        shovel_mid();
 {
        cob_count = 0;
        shovel_mid();

+       servo_carry_open();

        servo_door_open();
        time_wait_ms(2000);
        shovel_down();
        servo_door_close();
        servo_door_open();
        time_wait_ms(2000);
        shovel_down();
        servo_door_close();

+       servo_carry_close();

 }
 
 /* main state machine */
 }
 
 /* main state machine */


@@ -246,6 +266,7 @@ void state_init(void)
        vt100_init(&local_vt100);
        shovel_down();
        servo_door_close();
        vt100_init(&local_vt100);
        shovel_down();
        servo_door_close();

+       servo_carry_close();

        spickle_pack(I2C_LEFT_SIDE);
        spickle_pack(I2C_RIGHT_SIDE);
        state_mode = 0;
        spickle_pack(I2C_LEFT_SIDE);
        spickle_pack(I2C_RIGHT_SIDE);
        state_mode = 0;