cobboard

diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c
index cebcd2aaf05d1b0fed3e22f97c4d3f66c99501ba..79f91cd8cd091b304a2f315298afbc05d7dadf23 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 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)
 {
-       /* 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)
 {
-       /* 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)

diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index 17d8e5440150c0d3867d9eb221796daa2618bb3c..f9c0b7da6ddaa433beba7eb13a77899879dd69b8 100644 (file)
--- a/projects/microb2010/cobboard/cs.c
+++ b/projects/microb2010/cobboard/cs.c
@@ -47,13 +47,20 @@
 #include "actuator.h"
 #include "spickle.h"
 
+#define DEBUG_CPLD
+
 /* 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);
        }
+#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)
@@ -63,6 +70,31 @@ static void do_cs(__attribute__((unused)) void *dummy)
                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);
@@ -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 "
-             "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));
@@ -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 "
-                     "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));
@@ -147,14 +179,19 @@ void microb_cs_init(void)
        /* ---- 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);
 
+       /* 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_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);
@@ -170,7 +207,7 @@ void microb_cs_init(void)
        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);
 }

diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c
index 39318d9facfb733e61a4ac6c2423970f4d4768e4..62103c813fa2faefaff7534219b5883a411f07db 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>
- * 
+ *
  *  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
- * SUT 1-0 : 10 
+ * SUT 1-0 : 10
  * 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");
-       
+
        /* never returns */
 }
 
@@ -122,7 +122,7 @@ void do_led_blink(__attribute__((unused)) void *dummy)
                LED1_ON();
        else
                LED1_OFF();
-       
+
        a = !a;
 #endif
 }
@@ -183,6 +183,8 @@ int main(void)
        error_register_notice(mylog);
        error_register_debug(mylog);
 
+       wait_ms(3000);
+
        /* 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 */
-       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);
-       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);
-       
+
        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 */
-       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);
-       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);
@@ -225,12 +227,12 @@ int main(void)
                      NULL, 0);
        PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);
-       
+
        /* 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();

diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h
index a0dbac82f45f57475ce79adced56c27a8bee51b3..0d07a9c599cc271103714d9c2cbbf40f90472802 100755 (executable)
--- a/projects/microb2010/cobboard/main.h
+++ b/projects/microb2010/cobboard/main.h
@@ -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
@@ -51,6 +51,8 @@
 #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)
@@ -92,7 +94,7 @@ struct genboard {
        struct pwm_ng servo2;
        struct pwm_ng servo3;
        struct pwm_ng servo4;
-       
+
        /* ax12 interface */
        AX12 ax12;
 
@@ -104,6 +106,7 @@ struct genboard {
 
 struct cs_block {
        uint8_t on;
+       int32_t prev;
         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 eec58b174f40ba44a25dc1aa177ac1dfe7a70b3f..b820d9873674bdf2b64ad2567cb12c4667a8bf48 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 */
-       [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 */

diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h
index 6f477428828a910365d8b01d666f8211469891ad..4e634fcbb8fb5aefda7e51bc1e33d517dd139e8f 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
-#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

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index 3686eb70173a23c3fdac1f9ae641b701cd760e6c..62732c208cdfa24589dcba374bb5144396a3ae1c 100644 (file)
--- 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
@@ -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;
-       
+
        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 2903a4a0d0b7f0fd5de90e0b979f898e796609c8..33084050186c3e0e9406288604d5c0dd3e98812a 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 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)
 {
@@ -163,23 +170,34 @@ static void state_do_harvest(uint8_t 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);
-       time_wait_ms(delay);
+
+       WAIT_COND_OR_TIMEOUT(state_cob_inside(), delay);
 
        /* 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();
+
+       /* 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();
@@ -191,10 +209,12 @@ static void state_do_eject(void)
 {
        cob_count = 0;
        shovel_mid();
+       servo_carry_open();
        servo_door_open();
        time_wait_ms(2000);
        shovel_down();
        servo_door_close();
+       servo_carry_close();
 }
 
 /* main state machine */
@@ -246,6 +266,7 @@ void state_init(void)
        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;