ballboard blocking detection

diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index 7365427..d745fa0 100644 (file)
--- a/projects/microb2010/ballboard/actuator.c
+++ b/projects/microb2010/ballboard/actuator.c
@@ -40,11 +40,12 @@
 
 #include <rdline.h>
 
 
 #include <rdline.h>
 

+#include "actuator.h"

 #include "main.h"
 
 #include "main.h"
 

-#define ROLLER_ON      -1200
+#define ROLLER_ON      -ROLLER_SPEED

 #define ROLLER_OFF     0
 #define ROLLER_OFF     0

-#define ROLLER_REVERSE 1200
+#define ROLLER_REVERSE ROLLER_SPEED

 
 #define FORKROT_DEPLOYED -50000
 #define FORKROT_PACKED   0
 
 #define FORKROT_DEPLOYED -50000
 #define FORKROT_PACKED   0

diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h
index 34bd4ef..039cb1d 100644 (file)
--- a/projects/microb2010/ballboard/actuator.h
+++ b/projects/microb2010/ballboard/actuator.h
@@ -19,6 +19,8 @@
  *
  */
 
  *
  */
 

+#define ROLLER_SPEED 1200
+

 void roller_on(void);
 void roller_off(void);
 void roller_reverse(void);
 void roller_on(void);
 void roller_off(void);
 void roller_reverse(void);

diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c
index f6edc88..ed5c0ff 100644 (file)
--- a/projects/microb2010/ballboard/cs.c
+++ b/projects/microb2010/ballboard/cs.c
@@ -73,14 +73,15 @@ static void do_cs(void *dummy)
                        cs_manage(&ballboard.forkrot.cs);
        }
        if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) {
                        cs_manage(&ballboard.forkrot.cs);
        }
        if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) {

-               bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs);

                bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
                bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs);
                bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
                bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs);

+               bd_manage_from_speed_cmd(&ballboard.roller.bd,
+                                        cs_get_filtered_feedback(&ballboard.roller.cs),
+                                        cs_get_out(&ballboard.roller.cs));

 
                /* urgent case: stop power on blocking */
                if (ballboard.flags & DO_ERRBLOCKING) {
 
                /* urgent case: stop power on blocking */
                if (ballboard.flags & DO_ERRBLOCKING) {

-                       if (bd_get(&ballboard.roller.bd) ||
-                           bd_get(&ballboard.forktrans.bd) ||
+                       if (bd_get(&ballboard.forktrans.bd) ||

                            bd_get(&ballboard.forkrot.bd)) {
                                printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
                                ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
                            bd_get(&ballboard.forkrot.bd)) {
                                printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
                                ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);


@@ -137,8 +138,9 @@ void microb_cs_init(void)
 
        /* Blocking detection */
        bd_init(&ballboard.roller.bd);
 
        /* Blocking detection */
        bd_init(&ballboard.roller.bd);

-       bd_set_speed_threshold(&ballboard.roller.bd, 150);
-       bd_set_current_thresholds(&ballboard.roller.bd, 500, 8000, 1000000, 200);
+#define ROLLER_SPEED_THRES (ROLLER_SPEED * 0.75)
+       bd_set_speed_threshold(&ballboard.roller.bd, ROLLER_SPEED_THRES);
+       bd_set_current_thresholds(&ballboard.roller.bd, 500, 1500, 1000000, 20);

 
        /* ---- CS forktrans */
        /* PID */
 
        /* ---- CS forktrans */
        /* PID */

diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c
index 3f5a2eb..36bbf96 100644 (file)
--- a/projects/microb2010/ballboard/i2c_protocol.c
+++ b/projects/microb2010/ballboard/i2c_protocol.c
@@ -1,6 +1,6 @@
 /*
 /*

- *  Copyright Droids Corporation (2007)
- * 
+ *  Copyright Droids Corporation (2010)
+ *

  *  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


@@ -74,7 +74,7 @@ void i2c_send_status(void)
        struct i2c_ans_ballboard_status ans;
        i2c_flush();
        ans.hdr.cmd =  I2C_ANS_BALLBOARD_STATUS;
        struct i2c_ans_ballboard_status ans;
        i2c_flush();
        ans.hdr.cmd =  I2C_ANS_BALLBOARD_STATUS;

-       ans.status = 0x55; /* XXX */
+       ans.status = state_get_status();

        ans.ball_count = state_get_ball_count();
        ans.lcob = cob_detect_left();
        ans.rcob = cob_detect_right();
        ans.ball_count = state_get_ball_count();
        ans.lcob = cob_detect_left();
        ans.rcob = cob_detect_right();

diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h
index a267d78..68ecf97 100755 (executable)
--- a/projects/microb2010/ballboard/main.h
+++ b/projects/microb2010/ballboard/main.h
@@ -133,7 +133,7 @@ extern struct ballboard ballboard;
 /* start the bootloader */
 void bootloader(void);
 
 /* start the bootloader */
 void bootloader(void);
 

-#define wait_cond_or_timeout(cond, timeout)                   \
+#define WAIT_COND_OR_TIMEOUT(cond, timeout)                   \

 ({                                                            \
         microseconds __us = time_get_us2();                   \
         uint8_t __ret = 1;                                    \
 ({                                                            \
         microseconds __us = time_get_us2();                   \
         uint8_t __ret = 1;                                    \

diff --git a/projects/microb2010/ballboard/sensor.c b/projects/microb2010/ballboard/sensor.c
index 92b1c47..a3a705b 100644 (file)
--- a/projects/microb2010/ballboard/sensor.c
+++ b/projects/microb2010/ballboard/sensor.c
@@ -146,8 +146,8 @@ struct sensor_filter {
  * CAP 1
  */
 static struct sensor_filter sensor_filter[SENSOR_MAX] = {
  * CAP 1
  */
 static struct sensor_filter sensor_filter[SENSOR_MAX] = {

-       [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */
-       [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
+       [S_HIGH_BARRIER] = { 20, 0, 1, 19, 0, 1 }, /* 0 */
+       [S_LOW_BARRIER] = { 50, 0, 1, 1, 0, 0 }, /* 1 */

        [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */
        [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
        [S_R_IR] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
        [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */
        [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
        [S_R_IR] = { 1, 0, 0, 1, 0, 0 }, /* 4 */

diff --git a/projects/microb2010/ballboard/sensor.h b/projects/microb2010/ballboard/sensor.h
index 0d047d5..cb09f30 100644 (file)
--- a/projects/microb2010/ballboard/sensor.h
+++ b/projects/microb2010/ballboard/sensor.h
@@ -28,8 +28,8 @@
 #define ADC_MAX       4
 
 /* synchronize with sensor.c */
 #define ADC_MAX       4
 
 /* synchronize with sensor.c */

-#define S_CAP1         0
-#define S_CAP2         1
+#define S_HIGH_BARRIER 0
+#define S_LOW_BARRIER  1

 #define S_CAP3         2
 #define S_CAP4         3
 #define S_R_IR         4
 #define S_CAP3         2
 #define S_CAP4         3
 #define S_R_IR         4

diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index f9a5f8a..a322d17 100644 (file)
--- a/projects/microb2010/ballboard/state.c
+++ b/projects/microb2010/ballboard/state.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


@@ -56,6 +56,7 @@
 
 static struct vt100 local_vt100;
 static volatile uint8_t state_mode;
 
 static struct vt100 local_vt100;
 static volatile uint8_t state_mode;

+static volatile uint8_t state_status;

 static volatile uint8_t ball_count;
 
 /* short aliases */
 static volatile uint8_t ball_count;
 
 /* short aliases */


@@ -122,23 +123,81 @@ static uint8_t state_want_exit(void)
        return 0;
 }
 
        return 0;
 }
 

-uint8_t state_get_mode(void)
+uint8_t state_get_status(void)

 {
 {

-       return state_mode;
+       return state_status;

 }
 
 /* harvest balls from area */
 static void state_do_harvest(void)
 {
        //state_debug_wait_key_pressed();
 }
 
 /* harvest balls from area */
 static void state_do_harvest(void)
 {
        //state_debug_wait_key_pressed();

-       roller_on();
+
+       if (bd_get(&ballboard.roller.bd)) {
+               STMCH_DEBUG("%s(): roller blocked", __FUNCTION__);
+               roller_reverse();
+               time_wait_ms(500);
+               bd_reset(&ballboard.roller.bd);
+               STMCH_DEBUG("%s(): roller restart", __FUNCTION__);
+               return;
+       }
+
+       /* deduct ball count */
+       if (sensor_get(S_LOW_BARRIER) && !sensor_get(S_HIGH_BARRIER))
+               ball_count = 1;
+       else if (sensor_get(S_LOW_BARRIER) && sensor_get(S_HIGH_BARRIER))
+               ball_count = 3;
+
+       if (sensor_get(S_HIGH_BARRIER))
+               roller_off();
+       else
+               roller_on();

 }
 
 /* eject balls */
 static void state_do_eject(void)
 {
 }
 
 /* eject balls */
 static void state_do_eject(void)
 {

-       roller_reverse();
-       time_wait_ms(2000);
+       uint8_t i, blocked;
+       microseconds us;
+
+       for (i = 0; i < 3; i ++) {
+
+               roller_reverse();
+
+               us = time_get_us2();
+               blocked = 0;
+
+               while (1) {
+
+                       /* no more balls (sensor is heavily filtered) */
+                       if (!sensor_get(S_LOW_BARRIER)) {
+                               STMCH_DEBUG("%s(): no more balls", __FUNCTION__);
+                               break;
+                       }
+
+                       /* timeout */
+                       if ((time_get_us2() - us) > 1000UL * 1000UL) {
+                               STMCH_DEBUG("%s(): eject timeout", __FUNCTION__);
+                               blocked = 1;
+                               break;
+                       }
+
+                       /* blocking ! */
+                       if (bd_get(&ballboard.roller.bd)) {
+                               blocked = 1;
+                               break;
+                       }
+               }
+
+               if (!blocked)
+                       break;
+
+               STMCH_DEBUG("%s(): roller blocked", __FUNCTION__);
+               roller_on();
+               time_wait_ms(500);
+               bd_reset(&ballboard.roller.bd);
+               STMCH_DEBUG("%s(): roller restart", __FUNCTION__);
+       }

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


@@ -158,19 +217,24 @@ void state_machine(void)
                case INIT:
                        state_init();
                        state_mode = OFF;
                case INIT:
                        state_init();
                        state_mode = OFF;

+                       state_status = I2C_BALLBOARD_STATUS_F_READY;

                        break;
 
                case OFF:
                        break;
 
                case OFF:

+                       state_status = I2C_BALLBOARD_STATUS_F_READY;

                        roller_off();
                        break;
 
                case HARVEST:
                        roller_off();
                        break;
 
                case HARVEST:

+                       state_status = I2C_BALLBOARD_STATUS_F_READY;

                        state_do_harvest();
                        break;
 
                case EJECT:
                        state_do_harvest();
                        break;
 
                case EJECT:

-                       state_mode = HARVEST;
+                       state_status = I2C_BALLBOARD_STATUS_F_BUSY;

                        state_do_eject();
                        state_do_eject();

+                       state_status = I2C_BALLBOARD_STATUS_F_READY;
+                       state_mode = HARVEST;

                        break;
 
                default:
                        break;
 
                default:


@@ -183,5 +247,6 @@ void state_init(void)
 {
        vt100_init(&local_vt100);
        state_mode = 0;
 {
        vt100_init(&local_vt100);
        state_mode = 0;

+       state_status = I2C_BALLBOARD_STATUS_F_READY;

        ball_count = 0;
 }
        ball_count = 0;
 }

diff --git a/projects/microb2010/ballboard/state.h b/projects/microb2010/ballboard/state.h
index 9495ecf..777b086 100644 (file)
--- a/projects/microb2010/ballboard/state.h
+++ b/projects/microb2010/ballboard/state.h
@@ -26,7 +26,7 @@
 int8_t state_set_mode(uint8_t mode);
 
 /* get current state */
 int8_t state_set_mode(uint8_t mode);
 
 /* get current state */

-uint8_t state_get_mode(void);
+uint8_t state_get_status(void);

 
 uint8_t state_get_ball_count(void);
 
 
 uint8_t state_get_ball_count(void);