From 063af91cd38fceba6cbcf3a9b8abd4c48684d990 Mon Sep 17 00:00:00 2001 From: zer0 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 +#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.20.1