|
c't Projekte - c't-Bot und c't-Sim -
Mailinglisten
[Voriger (Datum)]
[Nächster (Datum)]
[Voriger (Thread)]
[Nächster (Thread)]
[Nach Datum][Nach Thread]
Absender: Harald W. Leschner
Datum: Mo, 15.09.2008 19:37:56
In-reply-to:
<000001c91695$89998e50$0200a8c0@mexpnew>
References:
<000001c91695$89998e50$0200a8c0@mexpnew>
Hallo liebe Bot-Entwickler,
vielleicht kann mir einer von Euch weiterhelfen: Ich habe hier ein paar
Routinen für die Anbindung einer CMUcam3 Camera an den Bot gebastelt und
komme gerade nicht mehr weiter...
Ich betreibe eine CMUcam3 (Rev.1.0b) mit der cmucam2 Emulation, habe ein
paar Dinge geändert aber eigentlich ist die Kamera so, wie sie vom
Hersteller geliefert wurde. Die Baudrate habe ich auf 38400 eingestellt,
weil ich die Kamera zuvor mit einer Software-UART an einem ATmega32 für
Entwicklungszwecke betrieben hatte. Hatte mir dafür die Basic-Hardware
des Bots auf einem Steckbrett nachgebaut und die HW-UART zum Debuggen
auf den Sim zweckentfremdet. Hat auch prima geklappt.
Jetzt habe ich die halb-fertigen Routinen auf den echten Bot übertragen.
Die Kamera wird nun über die Hardware UART des ct-Bots mit ATmega644
betrieben, habe dafür alle Logs und andere Kommunikationsmodule erstmal
deaktiviert, damit die Schnittstelle frei ist. Idee war ja, dem Bot
dafür später ein 2. Board zu spendieren, aber erstmal will ich die
Kamera so "rudimentär" am Bot betreiben, also geht es hier nur um die
simpelste Anbindung, trotzdem gibt es ein paar Problemchen...
Folgendes passiert: Die Kamera wird korrekt initialisiert und man kann
mittels eines eigenen Screens und ein paar Menupunkten (0=IDLE,
1=TRACKING ...) die Funktion überwachen. Die empfangenen Daten werden
dann in einem Kontainer gespeichert, angezeigt und stimmen auch so.
Insgesamt funktioniert es ganz gut, manchmal stürzt der Bot aber auch ab...
Nun zum eigentlichen Problem: Lässt man die Kamera im IDLE-Mode, fährt
der Bot normal. Aktiviert man die Trackingfunktion, scheint die
Geschwindigkeitsregelung völlig rumzuspinnen und der Bot dreht total ab,
ruckelt und stottert. Stoppt man das Tracking, ist es manchmal wieder
ok, manchmal sind dann aber auch nach einem Reset oder An/Aus die PWM
Werte völlig zerschossen, man muss den Bot neu programmieren.
Ich nehme an, die UART des Bots arbeitet absolut zuverlässig, denn bei
Übertragungen an den Sim fährt er ja auch normal. Kann sich einer von
Euch mal den Code angucken und mir sagen, ob vielleicht ein Fehler drin
ist, den der Compiler nicht erkennt, oder überhaupt vielleicht Ideen für
das geschilderte Verhalten geben?
Vielen Dank
Hari
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ct-Bot.c
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ct-Bot.c (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ct-Bot.c (working copy)
@@ -33,6 +33,7 @@
#include "i2c.h"
#include "twi.h"
#include "sp03.h"
+ #include "cmucam3.h"
#endif
#ifdef PC
@@ -160,6 +161,9 @@
#ifdef DISPLAY_AVAILABLE
gui_init();
#endif
+ #ifdef CMUCAM3_AVAILABLE
+ cmucam3_init();
+ #endif
}
#ifdef MCU
@@ -275,6 +279,9 @@
//display_printf("%6u", (uint16)(time_end - time_ticks));
uart_gui = 0; // naechstes Mal wieder mit GUI anfangen
}
+ #ifdef CMUCAM3_AVAILABLE
+ camera_handler(); // CMUcam3 Daten und Menu bearbeiten
+ #endif
}
//static uint16 old_time = 0;
//register uint16 time_ticks = TIMER_GET_TICKCOUNT_16;
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/Makefile
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/Makefile (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/Makefile (working copy)
@@ -48,7 +48,7 @@
MSG_DEVICE = Target device is $(DEVICE)
# List C source files here. (C dependencies are automatically generated.)
-SRCMCU = mcu/adc.c mcu/bootloader.c mcu/bot-2-pc.c mcu/cmps03.c mcu/delay.c mcu/display.c mcu/ena.c mcu/i2c.c mcu/ir-rc5.c mcu/led.c mcu/mini-fat.c mcu/minilog.c mcu/mmc.c mcu/motor-low.c mcu/mouse.c mcu/os_scheduler.c mcu/os_thread.c mcu/sensor-low.c mcu/shift.c mcu/sp03.c mcu/spi.c mcu/srf10.c mcu/timer-low.c mcu/twi.c mcu/uart.c
+SRCMCU = mcu/adc.c mcu/bootloader.c mcu/bot-2-pc.c mcu/cmps03.c mcu/delay.c mcu/display.c mcu/ena.c mcu/i2c.c mcu/ir-rc5.c mcu/led.c mcu/mini-fat.c mcu/minilog.c mcu/mmc.c mcu/motor-low.c mcu/mouse.c mcu/os_scheduler.c mcu/os_thread.c mcu/sensor-low.c mcu/shift.c mcu/sp03.c mcu/spi.c mcu/srf10.c mcu/timer-low.c mcu/twi.c mcu/uart.c mcu/cmucam3.c
SRCPC = pc/bot-2-sim.c pc/cmd-tools_pc.c pc/delay_pc.c pc/display_pc.c pc/eeprom_pc.c pc/ir-rc5_pc.c pc/led_pc.c pc/mini-fat.c pc/mmc-emu_pc.c pc/motor-low_pc.c pc/mouse_pc.c pc/os_thread_pc.c pc/sensor-low_pc.c pc/tcp-server.c pc/tcp.c
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ct-Bot.h
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ct-Bot.h (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ct-Bot.h (working copy)
@@ -79,6 +79,7 @@
//#define SRF10_AVAILABLE /*!< Ultraschallsensor SRF10 vorhanden */
//#define CMPS03_AVAILABLE /*!< Kompass CMPS03 vorhanden */
//#define SP03_AVAILABLE /*!< Sprachmodul SP03 vorhanden */
+#define CMUCAM3_AVAILABLE /*!< Kamera CMUcam3 vorhanden */
//#define MMC_AVAILABLE /*!< haben wir eine MMC/SD-Karte zur Verfuegung */
//#define SPI_AVAILABLE /*!< verwendet den Hardware-SPI-Modus des Controllers, um mit der MMC zu kommunizieren - Hinweise in mcu/mmc.c beachten! */
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/cmucam3.h
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/cmucam3.h (revision 0)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/cmucam3.h (revision 0)
@@ -0,0 +1,194 @@
+/*
+ * c't-Bot
+ *
+ * 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 (at your
+ * option) any later version.
+ * This program is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE. See the GNU General Public License for more details.
+ * You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the Free
+ * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307, USA.
+ *
+ */
+
+/*!
+ * @file cmucam3.h
+ * @brief Implementierung einer CMUcam3 Kamera
+ * @author Harald W. Leschner (hari@xxxxxxx)
+ * @date 14.09.08
+ */
+
+#include "ct-Bot.h"
+#ifdef CMUCAM3_AVAILABLE
+
+#include "global.h"
+
+#ifndef CMUCAM3_H_
+#define CMUCAM3_H_
+
+#ifdef MCU
+
+#include <avr/pgmspace.h>
+
+#define CC3_CMD_BUFFER_SIZE 30 // 42 Byte Befehlsbuffes
+
+#define CC3_READ_BUFFER_SIZE 40 // Anzahl gelesener Daten
+
+#define CC3_SYM_RETURN 0x0D // Return symbol from CAM
+#define CC3_SYM_CURSOR 0x3A // Cursor symbol from CAM
+
+/*!
+ * Datencontainer Definition
+ */
+struct cmucam3_T_packet {
+ uint8_t mx; /*!< mx - The middle of mass x value */
+ uint8_t my; /*!< my - The middle of mass y value */
+ uint8_t x1; /*!< x1 - The left most cornerâ??s x value */
+ uint8_t y1; /*!< y1 - The left most cornerâ??s y value */
+ uint8_t x2; /*!< x2 - The right most cornerâ??s y value */
+ uint8_t y2; /*!< y1 - The right most cornerâ??s y value */
+ uint8_t pixels; /*!< pixels â??Number of Pixels in the tracked region, scaled and capped at 255: (pixels+4)/8 */
+ uint8_t confidence; /*!< confidence -The (# of pixels / area)*256 of the bounded rectangle and capped at 255 */
+};
+
+struct cmucam3_H_packet {
+ uint8_t bin[28]; /*!< H bin0 bin1 bin2 bin3 â?¦ bin26 bin27 \r */
+};
+
+struct cmucam3_S_packet {
+ uint8_t Rmean; /*!< Rmean - the mean Red or Cr (approximates r-g) value in the current window */
+ uint8_t Gmean; /*!< Gmean - the mean Green or Y (approximates intensity) value found in the current window */
+ uint8_t Bmean; /*!< Bmean - the mean Blue or Cb (approximates b-g) found in the current window */
+ uint8_t Rdeviation; /*!< Rdeviation - the *deviation of red or Cr found in the current window */
+ uint8_t Gdeviation; /*!< Gdeviation- the *deviation of green or Y found in the current window */
+ uint8_t Bdeviation; /*!< Bdeviation- the *deviation of blue or Cb found in the current window */
+};
+
+struct cmucam3_retval {
+ enum { FAIL, NCK, ACK } ret_answ;
+ enum { STR, T, H, S, I } ret_type;
+ struct cmucam3_T_packet T;
+ struct cmucam3_H_packet H;
+ struct cmucam3_S_packet S;
+ char STR[20];
+};
+
+/*! Empfangene Daten */
+extern struct cmucam3_retval sensCMUcam3;
+
+/*! Status der Kamerakommunikation */
+extern uint8_t cc3_state;
+
+
+/*! Status der Kamerakommunikation */
+#define CC3_MENU_IDLE 0
+#define CC3_MENU_TRACKING 1
+#define CC3_MENU_NICKYES 2
+
+
+/*!
+ * @brief Schreibt einen Befehl in das Commandoregister
+ * @param format Format, wie beim printf
+ * @param ... Variable Argumentenliste, wie beim printf
+ */
+void cc2_flash_printf_cmd(const char *format, ...);
+
+/*!
+ * @brief Makro zum Senden eines Befehls an die CMUcam
+ * @param format Format, wie beim printf
+ * @param ... Variable Argumentenliste, wie beim printf
+ */
+#define cc3_send_cmd(format, args...) { \
+ static const char commando[] PROGMEM = format; \
+ cc3_flash_printf_cmd(commando, ## args); \
+ cc3_uart_write(cc3_cmd_buffer); \
+}
+
+/*!
+ * @brief Ausfuehren eines Camerabefehls und Auswertung der Antwort *CK mit Ausgabe.
+ * @see cc3_parse_retval()
+ * @return Antworttyp *CK oder Fail
+ *
+ * Diese Geschichte wird in zwei Schritten erledigt, A- und B-Teil:
+ * Der A-Teil sendet den Befehl, der zuvor im Befehlsbuffer gespeichert wurde an die Kamera und
+ * wertet das empfangene *CK aus. Wurde der Befehl erfolgreich von der Kamera empfangen und mit
+ * ACK bestaetigt, wird die Weiterverarbeitung der Antwort an cc3_parse_retval() uebergeben.
+ * Sonst, wenn ein NCK empfangen wurde, liefert die Funktion ein cc3_state = 1 und der Befehl muss
+ * nochmal gesendet werden. Wird kein typisches *CK empfangen, wird abgebrochen und ein Fehler ausgegeben.
+ *
+ * @see cc3_parse_retval()
+ *
+ */
+extern struct cmucam3_retval cc3_read_ACK(void);
+
+/*!
+ * @brief Verarbeitung der Antwort nach positivem ACK bis zum Cursorzeichen.
+ * @return Gelieferte Datenpakete
+ * B-Teil, der als Ergebnis die gelesenen Daten von der Kamera formatiert und in die T, S, H oder STR Container packt.
+ */
+extern struct cmucam3_retval cc3_parse_retval(void);
+
+/*!
+ * @brief Druckt die Sensorwerte der CMUcam3 auf dem Log
+ * @param sensCMUcam3 Datencontainer mit Kamerawerten
+ */
+extern void cc3_print_retval(struct cmucam3_retval sensCMUcam3);
+
+/*!
+ * @brief Displayinfos CMUcam3 ausgeben und GUI Menusteuerung
+ */
+extern void cmucam3_display(void);
+
+/*!
+ * @brief Initialisiert die Kamera im CMUcam2 Modus.
+ * @see http://www.cmucam.org
+ */
+extern uint8_t cmucam3_init(void);
+
+/*!
+ * @brief Hauptroutine der Kamerafunktionen, wird alle 50ms aufgerufen
+ * Hier werden die einzelnen Menupunkte von der GUI ausgewertet und als Kamerafunktionen weiterverarbeitet
+ */
+extern void camera_handler(void);
+
+/*!
+ * @brief Beendet aktuelle Befehle und setzt die Kamera in den IDLE-Modus
+ */
+extern void cmucam3_idle(void);
+
+/*!
+ * @brief Einfache Trackingfunktion
+ * Trackt einen tiefroten Gegenstand, fuer Testzwecke
+ */
+extern void cmucam3_track_color(void);
+
+/*!
+ * @brief Spielerei, laesst den Kamerakopf einmal nicken
+ */
+extern void cmucam3_nick_yes(void);
+
+/*!
+ * @brief Reset der Kamera und neue Initialisierung
+ */
+extern void cmucam3_reset(void);
+
+/*!
+ * @brief Schalter fuer die serielle Schnittstelle
+ */
+extern void cc3_uart_write(void *data);
+
+/*!
+ * @brief Schalter fuer die serielle Schnittstelle
+ * @return Gelesene Daten
+ */
+extern uint8_t cc3_uart_read(void);
+
+#endif //MCU
+#endif // CMUCAM3_AVAILABLE
+#endif //CMUCAM3_H_
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/uart.h
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/uart.h (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/uart.h (working copy)
@@ -33,7 +33,12 @@
#include <avr/io.h>
#include "fifo.h"
-#define BAUDRATE 57600 /*!< Baudrate fuer UART-Kommunikation */
+#ifdef CMUCAM3_AVAILABLE
+ #define BAUDRATE 38400 /*!< Baudrate zur CMUCAM3-Kommunikation */
+#else
+ #define BAUDRATE 57600 /*!< Baudrate fuer normale UART-Kommunikation */
+#endif
+
//#define BAUDRATE 115200 /*!< Baudrate fuer UART-Kommunikation */
#if BAUDRATE == 115200
#define UART_DOUBLESPEED // 2X-Mode, sonst Takt zu ungenau
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/fifo.h
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/fifo.h (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/include/fifo.h (working copy)
@@ -119,4 +119,14 @@
return data;
}
+/*!
+ * Liefert das naechste Byte aus der FIFO, bei leerer FIFO wird gewartet, bis das naechste Zeichen eintrifft.
+ */
+extern uint8 fifo_get_wait(fifo_t *f);
+
+/*!
+ * Liefert das naechste Byte aus der FIFO als int8 bzw. 0, falls die FIFO leer ist.
+ */
+extern uint8 fifo_get_nowait(fifo_t *f);
+
#endif // _FIFO_H_
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/mcu/cmucam3.c
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/mcu/cmucam3.c (revision 0)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/mcu/cmucam3.c (revision 0)
@@ -0,0 +1,656 @@
+/*
+ * c't-Bot
+ *
+ * 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 (at your
+ * option) any later version.
+ * This program is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE. See the GNU General Public License for more details.
+ * You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the Free
+ * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307, USA.
+ *
+ */
+
+/*!
+ * @file cmucam3.c
+ * @brief Implementierung einer CMUcam3 Kamera
+ *
+ * Diese Routinen senden Befehle an die CMUcam3 und empfangen die Datenpakete aus den Trackingberechnungen.
+ * Spezielle Container beinhalten die sog. T S und H Paketdaten und stellen die Ergebnisse praktisch dar.
+ *
+ * Alle Infos zur Kamera gibt es auf ===> http://www.cmucam.org/ <===
+ *
+ * Die Kommunikation hat folgendes Protokoll:
+ *
+ * - Ist die Camera bereit, hat sie nach dem letzten beendeten Befehl ein ":" gesendet, den Cursor.
+ * - Befehl an Camera senden, z.B. "GV + \r"
+ * - Hat die Camera verstanden, sendet sie ein "ACK\r" oder "NCK\r" falls Fehleingabe
+ * - Liefert der Befehl mehrere Daten, sind diese in T, S, oder H Pakete eingeteilt, die mit "\r" getrennt werden,
+ * sonst kommen einzelne Daten, die am Ende wieder ein ":" haben.
+ *
+ * Die speziellen Initialisierungsbefehle sind weiter unten beschrieben.
+ *
+ * Status: unfertig, ungetestet
+ *
+ * @author Harald W. Leschner (hari@xxxxxxx)
+ * @date 14.09.08
+ */
+
+/* TODO:
+ * - Testen...
+ */
+
+
+#ifdef MCU
+#include "ct-Bot.h"
+
+#ifdef CMUCAM3_AVAILABLE
+
+#include "global.h"
+#include "ui/available_screens.h"
+#include "display.h"
+#include "rc5.h"
+#include "rc5-codes.h"
+#include "sensor.h"
+
+#include "cmucam3.h"
+#include "uart.h"
+#include "fifo.h"
+#include "delay.h"
+#include "log.h"
+
+#include <avr/io.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <avr/eeprom.h>
+
+
+
+
+/*! Empfangene Daten */
+struct cmucam3_retval sensCMUcam3;
+
+/*! Status der Kamerakommunikation */
+uint8_t cc3_state = 0;
+
+/*! Puffer fuer das Zusammenstellen eines Commandos */
+static char cc3_cmd_buffer[CC3_CMD_BUFFER_SIZE];
+
+//static char cc3_rcv_buffer[CC3_READ_BUFFER_SIZE];
+char cc3_answ_buf[CC3_READ_BUFFER_SIZE - 3];
+
+/*! Status der aktiven Camerafunktion */
+static unsigned char cc3_function_active = CC3_MENU_IDLE;
+
+/*!
+ * @brief Daten aus dem Flash holen
+ * @param flash Adresse der Daten im Flash
+ * @param ram Adresse der Daten im RAM
+ * @param n Anzahl zu lesender Zeichen
+ */
+static void get_str_from_flash(const char* flash, char* ram, uint8 n) {
+ uint8 i;
+ /* Zeichen des Strings wortweise aus dem Flash holen */
+ uint16* p_ram = (uint16*)ram;
+ uint16* p_flash = (uint16*)flash;
+ for (i=0; i<n; i++){
+ uint16 tmp = pgm_read_word(p_flash++);
+ *(p_ram++) = tmp;
+ if ((uint8)tmp == 0 || (tmp & 0xFF00) == 0) break; // Stringende erreicht
+ }
+ *((char*)p_ram) = 0; // evtl. haben wir ein Byte zu viel gelesen, das korrigieren wir hier
+}
+
+/*!
+ * @brief Schreibt einen Befehl in das Commandoregister
+ * @param format Format, wie beim printf
+ * @param ... Variable Argumentenliste, wie beim printf
+ */
+void cc3_flash_printf_cmd(const char *format, ...) {
+ char flash_str[CC3_CMD_BUFFER_SIZE+4]; // bissel groesser, weil die % ja noch mit drin sind
+ get_str_from_flash(format, flash_str, CC3_CMD_BUFFER_SIZE/2+2); // String aus dem Flash holen
+
+ va_list args;
+ unsigned int len = strlen(cc3_cmd_buffer);
+
+ va_start(args, format);
+ vsnprintf(&cc3_cmd_buffer[len], CC3_CMD_BUFFER_SIZE - len, flash_str, args);
+ va_end(args);
+
+ return;
+}
+
+/*!
+ * @brief Ausfuehren eines Camerabefehls und Auswertung der Antwort *CK mit Ausgabe.
+ * @see cc3_parse_retval()
+ * @return Antworttyp *CK oder Fail
+ *
+ * Diese Geschichte wird in zwei Schritten erledigt, A- und B-Teil:
+ * Der A-Teil sendet den Befehl, der zuvor im Befehlsbuffer gespeichert wurde an die Kamera und
+ * wertet das empfangene *CK aus. Wurde der Befehl erfolgreich von der Kamera empfangen und mit
+ * ACK bestaetigt, wird die Weiterverarbeitung der Antwort an cc3_parse_retval() uebergeben.
+ * Sonst, wenn ein NCK empfangen wurde, liefert die Funktion ein cc3_state = 1 und der Befehl muss
+ * nochmal gesendet werden. Wird kein typisches *CK empfangen, wird abgebrochen und ein Fehler ausgegeben.
+ *
+ * @see cc3_parse_retval()
+ *
+ */
+struct cmucam3_retval cc3_read_ACK(void) {
+ int8_t read_byte = 0; // Anzahl gelesener Daten
+ uint8_t retry = 0; // Anzahl erfolgter Leseversuche
+
+ char read_buf[CC3_READ_BUFFER_SIZE]; // Empfangene Daten von UART1
+ uint8_t n = 0;
+
+ /*!< Befehl auf Cam ausfuehren */
+ cc3_uart_write("\r"); // Kommando ist im Buffer, jetzt ENTER senden um zu starten
+ cc3_state = 1; // Flag "Command gesendet"
+
+ //LOG_DEBUG("COMMAND: %s\n", cc3_cmd_buffer); // Zur Sicherheit nochmal Kommando anzeigen
+
+ /*!< Commandpuffer leeren */
+ strcpy(cc3_cmd_buffer, ""); // Mit leerem Puffer beginnen
+ strcpy(read_buf, " ");
+
+ /*!< Warten auf Cam Antwort */
+ while (cc3_state != 0){
+
+ /*!< Lesen des empfangenen *CK */
+ read_byte = cc3_uart_read(); // Zeichen in FIFO von UART auslesen
+ retry++;
+
+ if (read_byte != 0) { // Kopiere Daten in Empfangsbuffer
+ read_buf[n] = read_byte;
+ n++;
+
+ /*!< Hat nach RETURN erfolgreich gelesen */
+ //LOG_DEBUG("read %d bytes %X",n,read_byte);
+ //LOG_DEBUG("%s\n", read_buf);
+ }
+
+ /*!< Auswertung *CK Antwort der CMUcam */
+ if (0 == strncasecmp(read_buf, "ACK\r", 4)){ // Kamera da und versteht Befehle
+ sensCMUcam3.ret_answ = ACK; // Antwort kann weiterverarbeitet werden
+
+ // TODO: Auswertung *nach* dem ACK // Weiterverarbeitung siehe unten
+ sensCMUcam3 = cc3_parse_retval(); // weiter zum B-Teil
+ break;
+ }
+ else if (0 == strncasecmp(read_buf, "NCK\r:", 5)){ // Kamera da aber versteht nicht
+ sensCMUcam3.ret_answ = NCK; // Befehl muss nochmal gesendet werden
+ cc3_print_retval(sensCMUcam3);
+ cc3_state = 1;
+ break; // State bleibt 1
+ }
+ /*!< CMUcam antwortet nicht, Abbruch */
+ if(retry > 254) { // Kamera offensichtlich nicht da
+ sensCMUcam3.ret_answ = FAIL; // Ausgabe von Fehlermeldung
+ cc3_print_retval(sensCMUcam3);
+ LOG_DEBUG("no *CK");
+ cc3_state = 0;
+ break;
+ }
+ delay(1); //1ms warten
+
+ }
+ return sensCMUcam3;
+}
+
+/*!
+ * @brief Verarbeitung der Antwort nach positivem ACK bis zum Cursorzeichen.
+ * @return Gelieferte Datenpakete
+ * B-Teil, der als Ergebnis die gelesenen Daten von der Kamera formatiert und in die T, S, H oder STR Container packt.
+ */
+struct cmucam3_retval cc3_parse_retval(void) {
+ int8_t read_byte=0; // Anzahl gelesener Daten
+
+ char read_buf[CC3_READ_BUFFER_SIZE]; // Empfangene Daten von UART1
+ uint8_t n = 0, i = 0, j = 0;
+
+ /*!< Warten auf Cam Antwort */
+ while ( read_byte != CC3_SYM_CURSOR ){ // Daten vorhanden ?
+
+ /*!< Lesen des empfangenen Bytes */
+ read_byte = cc3_uart_read(); // 1 Byte aus FIFO von UART auslesen
+ //LOG_DEBUG("%d RESP: %X",n ,read_byte);
+
+ /*!< Alle Daten in Empfangspuffer */
+ if (read_byte != 0) { // Kopiere in Empfangsbuffer
+ read_buf[n] = read_byte;
+ n++;
+
+ /*!< Hat nach RETURN erfolgreich gelesen */
+ //LOG_DEBUG("read %d bytes %X",n,read_byte);
+ //LOG_DEBUG("%s\n", read_buf);
+ }
+ }
+
+ /*!< Daten bis Return auswerten */
+ char *token;
+ char *tmp;
+
+ token = strtok_r(read_buf, ":", &tmp );
+ strcpy(cc3_answ_buf, token);
+
+ //LOG_DEBUG("TOK %s", cc3_answ_buf);
+
+ /*!< Auswertung TSH oder STR Daten und verpacken in Packete */
+ char cur_num[20]; // Leseposition aktueller Daten
+ n = 0;
+ j = 0;
+
+ // Auswertung erstes Byte der Antwort
+ switch(cc3_answ_buf[0]) {
+ /*!< TRACKING T Typ Paket */
+ case 'T':
+ sensCMUcam3.ret_type = T;
+ for(i=2; i<strlen(cc3_answ_buf)+1; i++) {
+ if (isdigit(cc3_answ_buf[i])) {
+ cur_num[j] = cc3_answ_buf[i];
+ cur_num[j+1] = 0;
+ j++;
+ } else if (cc3_answ_buf[i] == ' ' || cc3_answ_buf[i] == 0) {
+ j = 0;
+ switch(n) {
+ case 0:
+ sensCMUcam3.T.mx = atoi(cur_num);
+ break;
+ case 1:
+ sensCMUcam3.T.my = atoi(cur_num);
+ break;
+ case 2:
+ sensCMUcam3.T.x1 = atoi(cur_num);
+ break;
+ case 3:
+ sensCMUcam3.T.y1 = atoi(cur_num);
+ break;
+ case 4:
+ sensCMUcam3.T.x2 = atoi(cur_num);
+ break;
+ case 5:
+ sensCMUcam3.T.y2 = atoi(cur_num);
+ break;
+ case 6:
+ sensCMUcam3.T.pixels = atoi(cur_num);
+ break;
+ case 7:
+ sensCMUcam3.T.confidence = atoi(cur_num);
+ break;
+ }
+ n++;
+ }
+ }
+ break;
+ /*!< STATISTIK S Typ Paket */
+ case 'S':
+ sensCMUcam3.ret_type = S;
+ for(i=2; i<strlen(cc3_answ_buf)+1; i++) {
+ if (isdigit(cc3_answ_buf[i])) {
+ cur_num[j] = cc3_answ_buf[i];
+ cur_num[j+1] = 0;
+ j++;
+ } else if (cc3_answ_buf[i] == ' ' || cc3_answ_buf[i] == 0) {
+ j = 0;
+ switch(n) {
+ case 0:
+ sensCMUcam3.S.Rmean = atoi(cur_num);
+ break;
+ case 1:
+ sensCMUcam3.S.Gmean = atoi(cur_num);
+ break;
+ case 2:
+ sensCMUcam3.S.Bmean = atoi(cur_num);
+ break;
+ case 3:
+ sensCMUcam3.S.Rdeviation = atoi(cur_num);
+ break;
+ case 4:
+ sensCMUcam3.S.Gdeviation = atoi(cur_num);
+ break;
+ case 5:
+ sensCMUcam3.S.Bdeviation = atoi(cur_num);
+ break;
+ }
+ n++;
+ }
+ }
+ break;
+ /*!< HISTOGRAMM H Typ Paket */
+ case 'H':
+ sensCMUcam3.ret_type = H;
+ break;
+ /*!< DEFAULT STR Typ Paket */
+ default:
+ sensCMUcam3.ret_type = STR;
+ strcpy(sensCMUcam3.STR, cc3_answ_buf);
+ break;
+ }// Ende switch TSH
+
+// /*!< Daten ausgeben, falls erfolgreich gelesen */
+// if (cc3_state != 0) {
+// LOG_DEBUG("COMMAND: %s\n", cc3_cmd_buffer);
+// cc3_print_retval(sensCMUcam3);
+// }
+
+ /*!< Flag Toggeln, frei fuer neue Befehle */
+ cc3_state = 0;
+ return sensCMUcam3;
+}
+
+
+/*!
+ * @brief Druckt die Sensorwerte der CMUcam3 auf dem Log
+ * @param sensCMUcam3 Datencontainer mit Kamerawerten
+ */
+void cc3_print_retval(struct cmucam3_retval sensCMUcam3) {
+ //
+ switch(sensCMUcam3.ret_answ) {
+ case FAIL:
+ LOG_DEBUG("return type FAIL\n");
+ break;
+ case NCK:
+ LOG_DEBUG("return type NCK\n");
+ break;
+ case ACK:
+ LOG_DEBUG("return type ACK\n");
+ break;
+ }
+ if(sensCMUcam3.ret_answ == ACK){
+ switch(sensCMUcam3.ret_type) {
+ case T:
+ LOG_DEBUG("return type T\n");
+ LOG_DEBUG(" mx = %d", sensCMUcam3.T.mx);
+ LOG_DEBUG(" my = %d", sensCMUcam3.T.my);
+ LOG_DEBUG(" x1 = %d", sensCMUcam3.T.x1);
+ LOG_DEBUG(" y1 = %d", sensCMUcam3.T.y1);
+ LOG_DEBUG(" x2 = %d", sensCMUcam3.T.x2);
+ LOG_DEBUG(" y2 = %d", sensCMUcam3.T.y2);
+ LOG_DEBUG(" pixels = %d", sensCMUcam3.T.pixels);
+ LOG_DEBUG(" confidence = %d\n", sensCMUcam3.T.confidence);
+ break;
+ case S:
+ /*!< S-Pakete hier verarbeiten */
+ LOG_DEBUG("return type S\n");
+ LOG_DEBUG(" Rmean = %d", sensCMUcam3.S.Rmean);
+ LOG_DEBUG(" Bmean = %d", sensCMUcam3.S.Bmean);
+ LOG_DEBUG(" Gmean = %d", sensCMUcam3.S.Gmean);
+ LOG_DEBUG(" Rdeviation = %d", sensCMUcam3.S.Rdeviation);
+ LOG_DEBUG(" Bdeviation = %d", sensCMUcam3.S.Bdeviation);
+ LOG_DEBUG(" Gdeviation = %d\n", sensCMUcam3.S.Gdeviation);
+ break;
+ case H:
+ /*!< TODO H-Pakete hier verarbeiten */
+ break;
+ case STR:
+ LOG_DEBUG("return type STR\n");
+ LOG_DEBUG("string = '%s'", sensCMUcam3.STR);
+ break;
+ case I:
+ // DUMMY wenn IDLE
+ break;
+ }
+ }
+}
+
+
+/*!
+ * @brief Initialisiert die Kamera im CMUcam2 Modus.
+ * @see http://www.cmucam.org
+ */
+uint8_t cmucam3_init(void) {
+
+ LOG_DEBUG("Initializing CMUcam3...");
+ cc3_state = 1;
+
+ // Checkung ob CMUcam da ist
+ while (cc3_state != 0){
+ sensCMUcam3 = cc3_read_ACK();
+ delay(5); //5ms warten
+ }
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Poll Mode auf single */
+ cc3_send_cmd("PM 1");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Servo Mask auf 0011 */
+ cc3_send_cmd("SM 3");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Servo Parameter Pan&Tilt */
+ cc3_send_cmd("SP 16 8 10 15 7 8");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Cameraversion auslesen */
+ cc3_send_cmd("GV");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< alles klar, Kamera ist Online */
+ LOG_DEBUG("CMUcam3 READY");
+ }
+
+ return 1;
+}
+
+
+
+
+#ifdef CMUCAM3_DISPLAY_AVAILABLE
+
+/*!
+ * @brief Displayinfos CMUcam3 ausgeben und GUI Menusteuerung
+ */
+void cmucam3_display(void){
+
+ // Zustand auf Display immer anzeigen
+ switch(sensCMUcam3.ret_answ) {
+ case FAIL:
+ display_cursor(1, 1);
+ display_printf("CMUcam3 NOT THERE...");
+ break;
+ case NCK:
+ display_cursor(1, 1);
+ display_printf("NCK:");
+ break;
+ case ACK:
+ display_cursor(1, 1);
+ display_printf("ACK:");
+ break;
+ }
+
+ if(sensCMUcam3.ret_answ == ACK){
+
+ switch(sensCMUcam3.ret_type) {
+ case T: // TRACKING PAKETE
+ display_printf("Color Tracking..");
+ display_cursor(2, 1);
+ display_printf("mx=%3.d", sensCMUcam3.T.mx);
+ display_cursor(3, 1);
+ display_printf("my=%3.d", sensCMUcam3.T.my);
+ display_cursor(2, 8);
+ display_printf("x1=%3.d", sensCMUcam3.T.x1);
+ display_cursor(3, 8);
+ display_printf("y1=%3.d", sensCMUcam3.T.y1);
+ display_cursor(2, 15);
+ display_printf("x2=%3.d", sensCMUcam3.T.x2);
+ display_cursor(3, 15);
+ display_printf("y2=%3.d", sensCMUcam3.T.y2);
+ display_cursor(4, 1);
+ display_printf("pixels=%3.d", sensCMUcam3.T.pixels);
+ display_cursor(4, 12);
+ display_printf("conf=%3.d", sensCMUcam3.T.confidence);
+ break;
+
+ case S: // STATISTIK PAKETE
+ /*!< TODO H-Pakete hier verarbeiten */
+ break;
+
+ case H: // HISTOGRAMM-PAKETE
+ /*!< TODO H-Pakete hier verarbeiten */
+ break;
+
+ case STR: // STRINGS
+ display_printf("%s", sensCMUcam3.STR);
+ break;
+
+ case I: // DUMMY WENN IDLE
+ display_printf("Camera Idle... ");
+ break;
+ }
+ }
+
+ /* RC-5 Menu-Handler */
+ switch (RC5_Code) {
+
+ case RC5_CODE_0:
+ cc3_function_active = CC3_MENU_IDLE;
+ RC5_Code = 254;
+ break;
+
+ case RC5_CODE_1:
+ cc3_function_active = CC3_MENU_TRACKING;
+ RC5_Code = 254;
+ break;
+
+ case RC5_CODE_2:
+ cc3_function_active = CC3_MENU_NICKYES;
+ RC5_Code = 254;
+ break;
+ }
+
+
+}
+#endif // CMUCAM3_DISPLAY_AVAILABLE
+
+
+/*!
+ * @brief Hauptroutine der Kamerafunktionen, wird alle 50ms aufgerufen
+ * Hier werden die einzelnen Menupunkte von der GUI ausgewertet und als Kamerafunktionen weiterverarbeitet
+ */
+void camera_handler(void) {
+
+ if (cc3_function_active == CC3_MENU_TRACKING) {
+ cmucam3_track_color();
+ }
+
+ else if (cc3_function_active == CC3_MENU_NICKYES) {
+ cmucam3_nick_yes();
+ }
+
+ else if (cc3_function_active == CC3_MENU_IDLE) {
+ sensCMUcam3.ret_type = I;
+ }
+}
+
+/*!
+ * @brief Beendet aktuelle Befehle und setzt die Kamera in den IDLE-Modus
+ */
+void cmucam3_idle(void){
+ cc3_send_cmd("");
+ sensCMUcam3 = cc3_read_ACK();
+ sensCMUcam3.ret_type = I;
+}
+
+/*!
+ * @brief Einfache Trackingfunktion
+ * Trackt einen tiefroten Gegenstand, fuer Testzwecke
+ */
+void cmucam3_track_color(void){
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Test Tracking - Befehl */
+ cc3_send_cmd("TC 215 254 0 40 0 40");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+ else {
+ cmucam3_idle();
+ }
+}
+
+/*!
+ * @brief Spielerei, laesst den Kamerakopf einmal nicken
+ */
+void cmucam3_nick_yes(void){
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Test Tracking - Befehl */
+ cc3_send_cmd("SV 1 128");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+ else {
+ cmucam3_idle();
+ }
+ delay(100);
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Test Tracking - Befehl */
+ cc3_send_cmd("SV 1 108");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+ else {
+ cmucam3_idle();
+ }
+ delay(100);
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< Test Tracking - Befehl */
+ cc3_send_cmd("SV 1 128");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+ else {
+ cmucam3_idle();
+ }
+ cc3_function_active = CC3_MENU_IDLE;
+}
+
+/*!
+ * @brief Reset der Kamera und neue Initialisierung
+ */
+void cmucam3_reset(void){
+
+ display_clear();
+
+ if (sensCMUcam3.ret_answ == ACK){
+ /*!< RESET - Befehl */
+ cc3_send_cmd("RS");
+ sensCMUcam3 = cc3_read_ACK();
+ }
+ else {
+ cmucam3_idle();
+ }
+ cmucam3_init();
+}
+
+/*!
+ * @brief Schalter fuer die serielle Schnittstelle
+ */
+void cc3_uart_write(void *data){
+
+ uart_write(data, strlen(data));
+
+}
+
+/*!
+ * @brief Schalter fuer die serielle Schnittstelle
+ * @return Gelesene Daten
+ */
+uint8_t cc3_uart_read(void){
+
+ return fifo_get_nowait(&infifo);
+}
+
+#endif // CMUCAM3_AVAILABLE
+#endif // MCU
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/fifo.c
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/fifo.c (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/fifo.c (working copy)
@@ -147,3 +147,19 @@
return length;
}
+
+/*!
+ * Liefert das naechste Byte aus der FIFO, bei leerer FIFO wird gewartet, bis das naechste Zeichen eintrifft.
+ */
+uint8 fifo_get_wait(fifo_t *f){
+ while (!f->count);
+ return _inline_fifo_get(f);
+}
+
+/*!
+ * Liefert das naechste Byte aus der FIFO als int8 bzw. 0, falls die FIFO leer ist.
+ */
+uint8 fifo_get_nowait(fifo_t *f){
+ if (!f->count) return 0;
+ return (uint8)_inline_fifo_get(f);
+}
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ui/gui.c
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ui/gui.c (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ui/gui.c (working copy)
@@ -44,6 +44,7 @@
#include <string.h>
#include "rc5-codes.h"
#include "eeprom.h"
+#include "cmucam3.h"
/*! Keymap fuer Keypad-Eingaben */
EEPROM uint8_t gui_keypad_table[][5] = {
@@ -227,6 +228,9 @@
/* MiniFAT wird vor GUI initialisiert und schreibt deshalb einfach auf's leere Display, der Dummy hier verhindert nur das Ueberschreiben in den anschliessenden Bot-Zyklen, damit man die Daten noch lesen kann */
register_screen(&mini_fat_display);
#endif
+ #ifdef CMUCAM3_DISPLAY_AVAILABLE
+ register_screen(&cmucam3_display);
+ #endif
#ifdef SENSOR_DISPLAY_AVAILABLE
register_screen(&sensor_display);
#endif
Index: C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ui/available_screens.h
===================================================================
--- C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ui/available_screens.h (revision 1481)
+++ C:/home/ct-roboter/ct-bot_WORK/ct-Bot (SVN)/ct-Bot_SVN/ui/available_screens.h (working copy)
@@ -45,6 +45,7 @@
#define DISPLAY_MAP_AVAILABLE /*!< Zeigt Map-Display an */
#define DISPLAY_TRANSPORT_PILLAR /*!< Steuerung Transport-Pillar-Verhalten auf diesem Screen */
#define DISPLAY_DRIVE_STACK_AVAILABLE /*!< Steuerung Stack-Verhalten auf diesem Screen */
+#define CMUCAM3_DISPLAY_AVAILABLE /*!< Ausgabe Infoscreen */
#ifndef SPEED_CONTROL_AVAILABLE
#undef DISPLAY_REGELUNG_AVAILABLE
@@ -74,6 +75,8 @@
#ifndef BEHAVIOUR_DRIVE_STACK_AVAILABLE
#undef DISPLAY_DRIVE_STACK_AVAILABLE
#endif
-
+#ifndef CMUCAM3_AVAILABLE
+ #undef CMUCAM3_DISPLAY_AVAILABLE
+#endif
#endif // DISPLAY_AVAILABLE
#endif // AVAILABLE_SCREENS_H_
|
|