ballboard blocking detection

diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index 7365427cd61d5625b1dfff9b0ec617046370edd2..d745fa08552aac92869d9b42e0a88b2dd8c517f5 100644 (file)
--- a/projects/microb2010/ballboard/actuator.c
+++ b/projects/microb2010/ballboard/actuator.c
@@ -40,11 +40,12 @@
 
 #include <rdline.h>
 
+#include "actuator.h"
 #include "main.h"
 
-#define ROLLER_ON      -1200
+#define ROLLER_ON      -ROLLER_SPEED
 #define ROLLER_OFF     0
-#define ROLLER_REVERSE 1200
+#define ROLLER_REVERSE ROLLER_SPEED
 
 #define FORKROT_DEPLOYED -50000
 #define FORKROT_PACKED   0

diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h
index 34bd4ef755e675b5f3b47f79f9660c2b0d4e1d25..039cb1d97755b1cc55aaf522e7c587a2b09acda1 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);

diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c
index f6edc88529a668d1cfb649cb5c3736b7afece9fe..ed5c0ff03eccc54a82c1b8dda4584b954a3fa6d2 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)) {
-               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_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) {
-                       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);
@@ -137,8 +138,9 @@ void microb_cs_init(void)
 
        /* 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 */

diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c
index 3f5a2ebc05604e48e674fa1ea3705383058fa24e..36bbf96517288549e836e295672177ceed0adf0d 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
@@ -74,7 +74,7 @@ void i2c_send_status(void)
        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();

diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h
index a267d78fce952199dee2f6dff7b0b58af01a5c3b..68ecf97808c68b2fb787879262959903bc7ad341 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);
 
-#define wait_cond_or_timeout(cond, timeout)                   \
+#define WAIT_COND_OR_TIMEOUT(cond, timeout)                   \
 ({                                                            \
         microseconds __us = time_get_us2();                   \
         uint8_t __ret = 1;                                    \

diff --git a/projects/microb2010/ballboard/sensor.c b/projects/microb2010/ballboard/sensor.c
index 92b1c47482dcab8e0aa7948b5cff76b58cb88306..a3a705b43eab402460bb571758625ef635a02ea5 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] = {
-       [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 */

diff --git a/projects/microb2010/ballboard/sensor.h b/projects/microb2010/ballboard/sensor.h
index 0d047d528813030022239e22d17fb0832b769ec9..cb09f30fb11131db60f049c5047c4dba37364b86 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 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

diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index f9a5f8a7f9fa9c9a9df8cf41886cb0cc2f07e477..a322d17c8615eb885d5c429099f48cbbe90eb086 100644 (file)
--- a/projects/microb2010/ballboard/state.c
+++ b/projects/microb2010/ballboard/state.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
@@ -56,6 +56,7 @@
 
 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 */
@@ -122,23 +123,81 @@ static uint8_t state_want_exit(void)
        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();
-       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)
 {
-       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 */
@@ -158,19 +217,24 @@ void state_machine(void)
                case INIT:
                        state_init();
                        state_mode = OFF;
+                       state_status = I2C_BALLBOARD_STATUS_F_READY;
                        break;
 
                case OFF:
+                       state_status = I2C_BALLBOARD_STATUS_F_READY;
                        roller_off();
                        break;
 
                case HARVEST:
+                       state_status = I2C_BALLBOARD_STATUS_F_READY;
                        state_do_harvest();
                        break;
 
                case EJECT:
-                       state_mode = HARVEST;
+                       state_status = I2C_BALLBOARD_STATUS_F_BUSY;
                        state_do_eject();
+                       state_status = I2C_BALLBOARD_STATUS_F_READY;
+                       state_mode = HARVEST;
                        break;
 
                default:
@@ -183,5 +247,6 @@ void state_init(void)
 {
        vt100_init(&local_vt100);
        state_mode = 0;
+       state_status = I2C_BALLBOARD_STATUS_F_READY;
        ball_count = 0;
 }

diff --git a/projects/microb2010/ballboard/state.h b/projects/microb2010/ballboard/state.h
index 9495ecfa269625643235a8fabd471ac7854486d4..777b0865278305e0a538692857b769a497539f7c 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 */
-uint8_t state_get_mode(void);
+uint8_t state_get_status(void);
 
 uint8_t state_get_ball_count(void);