From 063af91cd38fceba6cbcf3a9b8abd4c48684d990 Mon Sep 17 00:00:00 2001
From: zer0 <zer0@carbon.local>
Date: Tue, 4 May 2010 19:15:07 +0200
Subject: [PATCH] ballboard blocking detection
---
projects/microb2010/ballboard/actuator.c | 5 +-
projects/microb2010/ballboard/actuator.h | 2 +
projects/microb2010/ballboard/cs.c | 12 +--
projects/microb2010/ballboard/i2c_protocol.c | 6 +-
projects/microb2010/ballboard/main.h | 2 +-
projects/microb2010/ballboard/sensor.c | 4 +-
projects/microb2010/ballboard/sensor.h | 4 +-
projects/microb2010/ballboard/state.c | 81 ++++++++++++++++++--
projects/microb2010/ballboard/state.h | 2 +-
9 files changed, 94 insertions(+), 24 deletions(-)
diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index 7365427..d745fa0 100644
--- 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 34bd4ef..039cb1d 100644
--- 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 f6edc88..ed5c0ff 100644
--- 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 3f5a2eb..36bbf96 100644
--- 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 a267d78..68ecf97 100755
--- 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 92b1c47..a3a705b 100644
--- 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 0d047d5..cb09f30 100644
--- 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 f9a5f8a..a322d17 100644
--- 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 9495ecf..777b086 100644
--- 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);
--
2.39.5