Aus RN-Wissen.de
Dirk (Diskussion | Beiträge) K |
Dirk (Diskussion | Beiträge) K (→Beispiel 3,3V-Messung) |
||
| (446 dazwischenliegende Versionen von 2 Benutzern werden nicht angezeigt) | |||
| Zeile 1: | Zeile 1: | ||
| + | [[Bild:RP6_Multi-IO_V1.JPG|RP6 Multi-IO V1.0|right|500px]] | ||
=RP6 Multi IO Erweiterungsmodul= | =RP6 Multi IO Erweiterungsmodul= | ||
| Zeile 7: | Zeile 8: | ||
Diese fünf Platinen können bei [http://www.roboternetz.de/community/members/47148-fabqu fabqu] (oder [mailto:fabqu@web.de]) bestellt werden. Sie sind mit Lötstopplack (schwarz) sowie beidseitig mit einem Bestückungsdruck (in weiß) versehen. | Diese fünf Platinen können bei [http://www.roboternetz.de/community/members/47148-fabqu fabqu] (oder [mailto:fabqu@web.de]) bestellt werden. Sie sind mit Lötstopplack (schwarz) sowie beidseitig mit einem Bestückungsdruck (in weiß) versehen. | ||
Sie sind nicht aufgebaut, aber fabqu bietet an, die schwer einzulötenden SMD-ICs (und bei Bedarf auch alle anderen SMD-Bauteile) in einer einmaligen Sammelbestellung zu bestellen und einzulöten. | Sie sind nicht aufgebaut, aber fabqu bietet an, die schwer einzulötenden SMD-ICs (und bei Bedarf auch alle anderen SMD-Bauteile) in einer einmaligen Sammelbestellung zu bestellen und einzulöten. | ||
| + | |||
| + | Umfangreiche Dokumentation, sämtliche Beispielprogramme und Bibliotheken, Daten, Teilelisten und eine Lötanleitung finden sich in den [http://www.rn-wissen.de/index.php/RP6_Multi_IO_Projekt_-_Software#Weblinks Weblinks]. | ||
==Hardware== | ==Hardware== | ||
| − | '''Der Hardware-Artikel befindet sich [[RP6 Multi IO Projekt| | + | '''Der Hardware-Artikel befindet sich [[RP6 Multi IO Projekt|HIER]].''' |
| Zeile 105: | Zeile 108: | ||
| 8 || || GND || GND || | | 8 || || GND || GND || | ||
|- | |- | ||
| − | | 9 || PF4 || ADC4 || SHARP_L || I | + | | 9 || PF4 || ADC4 || SHARP_L / 3V3 || I |
|- | |- | ||
| 10 || || GND || GND || | | 10 || || GND || GND || | ||
|- | |- | ||
| − | | 11 || PF5 || ADC5 || SHARP_R || I | + | | 11 || PF5 || ADC5 || SHARP_R / TOUCH || I |
|- | |- | ||
| 12 || PF6 || ADC6 || SNAKE_R || I | | 12 || PF6 || ADC6 || SNAKE_R || I | ||
| Zeile 138: | Zeile 141: | ||
Zu (3): Siehe folgende Abbildung! | Zu (3): Siehe folgende Abbildung! | ||
| − | ''' | + | '''Standard-Jumperstellung für die M256''' |
[[Bild:MultiIO_JPdef_2.png|Jumper Standardstellung]] | [[Bild:MultiIO_JPdef_2.png|Jumper Standardstellung]] | ||
| − | ''Hinweis: Die Jumper sind orange eingezeichnet! Die Jumper der Stromversorgung wurden nicht berücksichtigt!'' | + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' |
| + | |||
| + | ''Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!! Die Jumper der Stromversorgung wurden nicht berücksichtigt!'' | ||
| − | Die Library (Software-Bibliothek) besteht aus drei Teilen: | + | Die '''Library''' (Software-Bibliothek) besteht aus drei Teilen: |
* Dem [[RP6_Multi_IO_Projekt_-_Software#Configuration Header|Configuration Header]] -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | * Dem [[RP6_Multi_IO_Projekt_-_Software#Configuration Header|Configuration Header]] -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | ||
* Dem [[RP6_Multi_IO_Projekt_-_Software#Library Header|Library Header]] -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library. | * Dem [[RP6_Multi_IO_Projekt_-_Software#Library Header|Library Header]] -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library. | ||
| Zeile 190: | Zeile 195: | ||
//#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address | //#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address | ||
| − | // ( | + | // (Shunt resistor, current and voltage adjustment!) |
| − | #define SHUNT_R 0.051 // | + | #define SHUNT_R 0.051 // R10 [Ohm] |
| − | #define | + | #define C_ADJUST 2.0 // Current adjust factor |
| − | #define | + | #define C_OFFSET 0.0 // Current offset [mA] |
| − | #define VBAT_ADJUST 3.2 // ( | + | #define VBAT_ADJUST 3.2 // (R13 + R14) / R14 |
| − | #define VSERVO_ADJUST 2.5 // ( | + | #define VSERVO_ADJUST 2.5 // (R32 + R22) / R22 |
/*****************************************************************************/ | /*****************************************************************************/ | ||
| Zeile 291: | Zeile 296: | ||
//#define ADC_MULTIIO_TOUCH ADC_5 // At ADC-M256 | //#define ADC_MULTIIO_TOUCH ADC_5 // At ADC-M256 | ||
| − | #define ADCVAL_NOTOUCH | + | #define ADCVAL_NOTOUCH 15 |
| − | #define ADCVAL_TOUCH | + | #define ADCVAL_TOUCH 1020 |
#define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) | #define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) | ||
| Zeile 299: | Zeile 304: | ||
#define ADC_MULTIIO_BUTTONS ADC_11 // At ADC-Mxxx | #define ADC_MULTIIO_BUTTONS ADC_11 // At ADC-Mxxx | ||
| − | #define ADCVAL_BUTTON1 | + | #define ADCVAL_BUTTON1 14 |
| − | #define ADCVAL_BUTTON2 | + | #define ADCVAL_BUTTON2 577 |
| − | #define ADCVAL_BUTTON3 | + | #define ADCVAL_BUTTON3 736 |
| − | #define ADCVAL_BUTTON4 | + | #define ADCVAL_BUTTON4 812 |
#define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) | #define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) | ||
#define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) | #define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) | ||
| Zeile 867: | Zeile 872: | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // MultiIO Project Board initialisation: | + | // MultiIO Project Board initialisation and shutdown: |
void multiio_init(void); | void multiio_init(void); | ||
| + | void multiio_shutdown(void); | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| Zeile 1.020: | Zeile 1.026: | ||
void LTC2990_calculate(void) | void LTC2990_calculate(void) | ||
{ | { | ||
| − | int16_t tmp | + | int16_t tmp; |
| − | double | + | double v2; |
tmp = (((vcs_tint_high & 0x1f) << 8) + vcs_tint_low) << 3; | tmp = (((vcs_tint_high & 0x1f) << 8) + vcs_tint_low) << 3; | ||
| Zeile 1.027: | Zeile 1.033: | ||
tint = tmp / 16.0; // Internal temperature [°C] | tint = tmp / 16.0; // Internal temperature [°C] | ||
| − | tmp = ((( | + | tmp = (((vcs_v2_high & 0x7f) << 8) + vcs_v2_low) << 1; |
tmp = tmp / 2; | tmp = tmp / 2; | ||
| − | |||
| − | |||
| − | |||
| − | |||
v2 = tmp * 0.01942; | v2 = tmp * 0.01942; | ||
| − | v2 *= | + | v2 *= C_ADJUST; // Current adjust factor |
| − | cbat = | + | cbat = v2 / SHUNT_R + C_OFFSET; // Battery current [mA] |
tmp = ((vcs_v3_high & 0x3f) << 8) + vcs_v3_low; | tmp = ((vcs_v3_high & 0x3f) << 8) + vcs_v3_low; | ||
| Zeile 1.803: | Zeile 1.805: | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // MultiIO Project Board initialisation: | + | // MultiIO Project Board initialisation and shutdown: |
/** | /** | ||
| Zeile 1.820: | Zeile 1.822: | ||
// Buzzer: | // Buzzer: | ||
Buzzer_init(); // Init buzzer | Buzzer_init(); // Init buzzer | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * If you don't use any function of the MultiIO | ||
| + | * Project Board, you can put the MultiIO into | ||
| + | * "SHUTDOWN MODE". In this mode the electric | ||
| + | * power consumption is very low to save energy. | ||
| + | * | ||
| + | */ | ||
| + | void multiio_shutdown(void) | ||
| + | { | ||
| + | // LEDs: | ||
| + | setMultiIOLEDs(0b0000); // MultiIO LEDs off | ||
| + | // Servo power: | ||
| + | setServoPower(0); // Servo power off | ||
| + | // Servo Controller: | ||
| + | PCA9685_shutdown(); // Shutdown PCA9685 | ||
| + | // Temperature Sensor: | ||
| + | TCN75_shutdown(); // Shutdown TCN75A | ||
} | } | ||
| Zeile 1.829: | Zeile 1.850: | ||
* **************************************************************************** | * **************************************************************************** | ||
* Changelog: | * Changelog: | ||
| − | * - v. 2.0 (release for board | + | * - v. 2.2 (shutdown mode added) 28.03.2013 by Dirk |
| + | * - v. 2.1 (LTC2990 current bug fixed) 26.03.2013 by Dirk | ||
| + | * - v. 2.0 (release for board V1.0) 17.02.2013 by Dirk | ||
* - v. 1.0 (initial release) 21.01.2013 by Dirk | * - v. 1.0 (initial release) 21.01.2013 by Dirk | ||
* | * | ||
| Zeile 1.840: | Zeile 1.863: | ||
======Erklärung====== | ======Erklärung====== | ||
| − | |||
| − | |||
=====Demo===== | =====Demo===== | ||
| Zeile 1.847: | Zeile 1.868: | ||
makefile: | makefile: | ||
<pre>... | <pre>... | ||
| − | TARGET = | + | TARGET = RP6M256_MultiIO_01 |
... | ... | ||
SRC += RP6M256_MultiIOLib.c | SRC += RP6M256_MultiIOLib.c | ||
...</pre> | ...</pre> | ||
| − | Datei | + | Datei RP6M256_MultiIO_01.c: |
<pre>/* | <pre>/* | ||
* **************************************************************************** | * **************************************************************************** | ||
| Zeile 1.978: | Zeile 1.999: | ||
rtc_time.hour = 12; // 12:00 | rtc_time.hour = 12; // 12:00 | ||
rtc_date.weekday = R_TH; | rtc_date.weekday = R_TH; | ||
| − | rtc_date.day = | + | rtc_date.day = 28; |
| − | rtc_date.month = | + | rtc_date.month = 3; |
| − | rtc_date.year = 2013; // Do, | + | rtc_date.year = 2013; // Do, 28.3.2013 |
DS1307_write(); | DS1307_write(); | ||
// Remove this, if RTC is set and running!!! | // Remove this, if RTC is set and running!!! | ||
| Zeile 2.000: | Zeile 2.021: | ||
// Buzzer test: | // Buzzer test: | ||
| − | + | soundBuzzer(Tone_Cis2, 300, 200); | |
| − | + | soundBuzzer(Tone_Fis2, 200, 100); | |
| − | + | soundBuzzer(Tone_Ais2, 100, 100); | |
| − | + | soundBuzzer(Tone_Dis3, 50, 100); | |
mSleep(1000); | mSleep(1000); | ||
| − | + | soundBuzzer(Tone_Dis3, 300, 200); | |
| − | + | soundBuzzer(Tone_Ais2, 200, 100); | |
| − | + | soundBuzzer(Tone_Fis2, 100, 100); | |
| − | + | soundBuzzer(Tone_Cis2, 50, 100); | |
while(true) | while(true) | ||
| Zeile 2.092: | Zeile 2.113: | ||
writeDouble_WIFI(vcc, 4, 1); | writeDouble_WIFI(vcc, 4, 1); | ||
writeString_WIFI("V\n"); | writeString_WIFI("V\n"); | ||
| + | |||
| + | // MultiIO shutdown: | ||
| + | if (pressedButtonNumber == 4) { | ||
| + | writeString_WIFI("\nPress button 1 for MultiIO SHUTDOWN"); | ||
| + | writeString_WIFI("\nor any other button to continue!!!\n"); | ||
| + | do { | ||
| + | mSleep(1); | ||
| + | releasedButtonNumber = checkReleasedButtonEvent(); | ||
| + | task_I2CTWI(); | ||
| + | } while (!releasedButtonNumber); | ||
| + | if (releasedButtonNumber == 1) { | ||
| + | writeString_WIFI("\nPlease wait for MultiIO SHUTDOWN...\n"); | ||
| + | multiio_shutdown(); | ||
| + | mSleep(3000); | ||
| + | writeString_WIFI("\nThe MultiIO now is in SHUTDOWN MODE!!!\n"); | ||
| + | mSleep(1000); | ||
| + | writeString_WIFI("\nRESET the M256 microcontroller now...\n\n"); | ||
| + | while(true) {}; | ||
| + | } | ||
| + | } | ||
setStopwatch1(0); | setStopwatch1(0); | ||
| Zeile 2.105: | Zeile 2.146: | ||
======Erklärung====== | ======Erklärung====== | ||
| − | ====Linienfolger Board | + | ====Linienfolger und Bumper Board Library==== |
| − | + | Diese [[Avr-gcc]] Library für das Linienfolger und das Bumper Board des Multi IO Projekts geht von folgenden Voraussetzungen aus: | |
| + | * Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt. | ||
| + | * Die M256 ist der I2C-Bus Master. | ||
| + | * Die I2C-Bus Geschwindigkeit beträgt 100 kHz. | ||
| + | * Alle Jumper auf der MultiIO sind in ihrer Standardstellung (1). | ||
| + | * Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden. | ||
| + | * Der Wannenstecker IO_Mxxx der MultiIO ist mit dem Wannenstecker IO_PWM/T2/T3 der M256 1:1 verbunden. | ||
| + | * Der Wannenstecker ADC_Mxxx der MultiIO ist mit dem Wannenstecker ADC_IO2/CMP der M256 1:1 verbunden. | ||
| + | * Der Wannenstecker ADC_M256 der MultiIO ist mit dem Wannenstecker ADC_IO1 der M256 1:1 verbunden. | ||
| + | * Das Linienfolger und/oder das Bumper Board ist an den Stecker "CNY70" (P_CNY) bzw. "Bumper" (P_BUMPER) der MultiIO angeschlossen. | ||
| − | + | Zu (1): Siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]! | |
| − | ''' | + | '''Linienfolger Sensoren (LFS) anschliessen''' |
| + | |||
| + | Will man die Linienfolger Sensoren (LFS) anstelle der 3,3V-Messung und des Touch Sensors nutzen (mit dieser Library für die M256), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block: | ||
[[Bild:MultiIO_Wahl-Jumper_2.PNG|LFS Jumper Stellung]] | [[Bild:MultiIO_Wahl-Jumper_2.PNG|LFS Jumper Stellung]] | ||
''Hinweis: Die Jumper sind orange eingezeichnet!'' | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | ||
| − | + | '''I2C Sensoren (z.B. SRF02) anschliessen''' | |
| − | + | ||
| − | + | ||
| − | + | ||
| − | + | ||
| − | + | Will man I2C Sensoren (z.B. SRF02) anstelle von analogen Sensoren (SHARP, Radar) auf dem Bumper Board nutzen (mit dieser Library für die M256), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block: | |
| − | + | [[Bild:MultiIO_Wahl-Jumper_2b.PNG|I2C Jumper Stellung]] | |
| − | + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | |
| − | + | ||
| − | ... | + | |
| − | + | Die '''Library''' (Software-Bibliothek) besteht aus zwei Teilen: | |
| − | ... | + | * Dem [[RP6_Multi_IO_Projekt_-_Software#Library Header_2|Library Header]] -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library. |
| + | * Der [[RP6_Multi_IO_Projekt_-_Software#Library Source_2|Library Source]] -> Das ist die eigentliche Library. | ||
| + | |||
| + | Zusätzlich wird auch der [[RP6_Multi_IO_Projekt_-_Software#Configuration Header|MultiIO Configuration Header]] eingebunden. Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | ||
| + | |||
| + | =====Library Header===== | ||
| + | Vor dem Kompilieren der Library muss eine Anpassung des Headers an die eigenen Hardware-Voraussetzungen erfolgen. | ||
| + | Dazu gibt es im Header fünf Definitionen: | ||
| + | * LFS -> Linienfolger (= "LFS") Sensoren werden benutzt | ||
| + | * FIVE_SENSORS -> Es werden alle 5 LFS Sensoren (CNY70) benutzt (sonst nur 3!) | ||
| + | * BUMPERS -> Die Bumper werden verwendet | ||
| + | * SHARP_SENSORS -> Es werden analoge SHARP Sensoren benutzt '''ODER''' | ||
| + | * SRF02_SENSORS -> Es werden SRF02 Ultraschall Sensoren (I2C) benutzt | ||
| + | Im nachfolgenden Listing sind ALLE Definitionen aktiv. Dies muss an die eigene Hardware angepaßt werden. Wenn z.B. SRF02 NICHT benutzt werden, muss die Zeile mit "SRF02_SENSORS" auskommentiert werden. Dazu setzt man "//" an den Zeilenanfang. Beispiel: | ||
| + | //#define SRF02_SENSORS // SRF02 ultrasonic sensors are used | ||
| − | Datei: | + | Datei: RP6M256_LFSBumperLib.h: |
<pre>/* | <pre>/* | ||
* **************************************************************************** | * **************************************************************************** | ||
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
* **************************************************************************** | * **************************************************************************** | ||
| − | * Example: RP6M256 | + | * Example: RP6M256 LFS Bumper Library |
* Author(s): Dirk | * Author(s): Dirk | ||
* **************************************************************************** | * **************************************************************************** | ||
* Description: | * Description: | ||
| − | * | + | * Header file for new LFS and Bumper Board library. |
| − | + | * | |
| − | + | ||
| − | + | ||
| − | + | ||
| − | + | ||
| − | + | ||
| − | * | + | |
* **************************************************************************** | * **************************************************************************** | ||
*/ | */ | ||
| + | |||
| + | #ifndef RP6M256_LFSBUMPERLIB_H | ||
| + | #define RP6M256_LFSBUMPERLIB_H | ||
| + | |||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // | + | // LFS Board components: |
| + | // - 3 or 5 reflective optical sensors (CNY70) | ||
| − | + | // Bumper Board components: | |
| − | + | // - 2 LEDs | |
| − | + | // - 2 bumpers | |
| + | // - 2 SHARP IR distance sensors (e.g. GP2Y0A02YK) | ||
| + | // - 2 SRF02 ultrasonic distance sensors (SRF02) | ||
| + | // - 2 Radar sensors (RSM-1650 based) | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | / | + | // Includes: |
| − | + | ||
| − | + | ||
| − | #include " | + | #include "RP6M256Lib.h" |
| + | #include "RP6I2CmasterTWI.h" | ||
| + | #include "RP6M256_MultiIO.h" | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // | + | // Line Following Sensor (LFS) Board: |
| − | //#define FIVE_SENSORS // Only, if all 5 CNY70s are used! | + | // Define the use of the LFS Board and the number of CNY70s (3 or 5) to be |
| + | // used here! | ||
| + | #define LFS // LFS Board is used | ||
| + | #define FIVE_SENSORS // Only, if all 5 CNY70s are used! | ||
| + | // --------------------------------------------------------------------------- | ||
#define CH_LFS_L 1 | #define CH_LFS_L 1 | ||
| Zeile 2.175: | Zeile 2.243: | ||
#define CH_LFS_R1 4 | #define CH_LFS_R1 4 | ||
#define CH_LFS_L1 5 | #define CH_LFS_L1 5 | ||
| + | |||
| + | #define ADCVAL_BLACKLINE 424 | ||
| + | #define ADCVAL_WHITEPAPER 994 | ||
| + | #define ADCVAL_LIMIT_BW ((ADCVAL_WHITEPAPER - ADCVAL_BLACKLINE) / 2 + ADCVAL_BLACKLINE) | ||
| + | |||
| + | #ifdef LFS | ||
| + | void LFS_init(void); | ||
| + | uint16_t getLFS(uint8_t channel); | ||
| + | void setLFSPower(uint8_t pwr); | ||
| + | #endif | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // | + | // Bumper Board: |
| + | // Define the devices that are built up on the Bumper Board here! | ||
| + | // Make a comment of a line belonging to a sensor that is NOT built | ||
| + | // up on or attached to the Bumper Board! | ||
| + | // IMPORTANT: It is only possible to use EITHER SHARP sensors OR SRF02 | ||
| + | // sensors OR Radar sensors on the Bumper Board!!! | ||
| + | // But in addition you may connect up to four SRF02 sensors | ||
| + | // to the MultiIO Board! | ||
| + | #define BUMPERS // Bumpers are used | ||
| + | #define SHARP_SENSORS // SHARP IR distance sensors are used | ||
| + | #define SRF02_SENSORS // SRF02 ultrasonic sensors are used | ||
| + | #define RADAR_SENSORS // RSM-1650 Radar sensors are used | ||
| + | // --------------------------------------------------------------------------- | ||
| + | |||
| + | #ifndef LEFT | ||
| + | #define LEFT 2 | ||
| + | #endif | ||
| + | #ifndef RIGHT | ||
| + | #define RIGHT 3 | ||
| + | #endif | ||
| + | |||
| + | // SRF02 sensors (2) on the Bumper Board: | ||
| + | #define I2C_MULTIIO_SRF02_L_ADR 0xe0 // Default | ||
| + | #define I2C_MULTIIO_SRF02_R_ADR 0xe2 | ||
| + | // SRF02 sensors (4) connected to the MultiIO Board: | ||
| + | #define I2C_MULTIIO_SRF02_1_ADR 0xe4 | ||
| + | #define I2C_MULTIIO_SRF02_2_ADR 0xe6 | ||
| + | #define I2C_MULTIIO_SRF02_3_ADR 0xe8 | ||
| + | #define I2C_MULTIIO_SRF02_4_ADR 0xea | ||
| + | // Other possible SRF02 I2C addresses: | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xec | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xee | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xf0 | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xf2 | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xf4 | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xf6 | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xf8 | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xfa | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xfc | ||
| + | //#define I2C_MULTIIO_SRF02_x_ADR 0xfe | ||
| + | |||
| + | // SRF02 registers: | ||
| + | #define SRF02_COMMAND 0 // Read: Software Revision | ||
| + | #define SRF02_RANGE_MSB 2 | ||
| + | #define SRF02_RANGE_LSB 3 | ||
| + | #define SRF02_AUTOTUNE_MIN_MSB 4 | ||
| + | #define SRF02_AUTOTUNE_MIN_LSB 5 | ||
| + | |||
| + | // SRF02 mode constants: | ||
| + | #define MODE_INCH 80 // Result in [inch] | ||
| + | #define MODE_CM 81 // Result in [cm] | ||
| + | #define MODE_US 82 // Result in [us] | ||
| + | #define MODE_SYNC_INCH 86 // Result in [inch] * | ||
| + | #define MODE_SYNC_CM 87 // Result in [cm] * | ||
| + | #define MODE_SYNC_US 88 // Result in [us] * | ||
| + | #define MODE_40KHZ_BURST 92 // Ping. No measurement! | ||
| + | // *) Fake ranging mode! | ||
| + | |||
| + | // Sonic speed constants in air (default: at 20°C): | ||
| + | //#define SONIC_SPEED 325.4 // [m/s] at -10°C | ||
| + | //#define SONIC_SPEED 328.5 // [m/s] at - 5°C | ||
| + | //#define SONIC_SPEED 331.5 // [m/s] at 0°C | ||
| + | //#define SONIC_SPEED 334.5 // [m/s] at 5°C | ||
| + | //#define SONIC_SPEED 337.5 // [m/s] at 10°C | ||
| + | //#define SONIC_SPEED 340.5 // [m/s] at 15°C | ||
| + | #define SONIC_SPEED 343.4 // [m/s] at 20°C | ||
| + | //#define SONIC_SPEED 346.3 // [m/s] at 25°C | ||
| + | //#define SONIC_SPEED 349.2 // [m/s] at 30°C | ||
| + | |||
| + | // SRF02 channels: | ||
| + | #define CH_SRF02_L 1 | ||
| + | #define CH_SRF02_R 2 | ||
| + | #define CH_SRF02_1 3 | ||
| + | #define CH_SRF02_2 4 | ||
| + | #define CH_SRF02_3 5 | ||
| + | #define CH_SRF02_4 6 | ||
| + | |||
| + | // LEDs (always used): | ||
| + | void LEDS_init(void); | ||
| + | void setbumperLEDL(uint8_t led); | ||
| + | void setbumperLEDR(uint8_t led); | ||
| + | void setbumperLEDs(uint8_t leds); | ||
| + | // Bumpers: | ||
| + | #ifdef BUMPERS | ||
| + | void MULTIIO_BUMPERS_init(void); | ||
| + | uint8_t getMULTIIO_BUMPER(uint8_t side); | ||
| + | extern uint8_t iobumper_l; | ||
| + | extern uint8_t iobumper_r; | ||
| + | void MULTIIO_BUMPERS_setStateChangedHandler(void (*bumperHandler)(void)); | ||
| + | void task_MULTIIO_BUMPERS(void); | ||
| + | #endif | ||
| + | // SHARP sensors: | ||
| + | #ifdef SHARP_SENSORS | ||
| + | void SHARPS_init(void); | ||
| + | extern uint16_t adcsharp_l; | ||
| + | extern uint16_t adcsharp_r; | ||
| + | uint16_t getSHARPSensor(uint8_t side); | ||
| + | extern double distsharp_l; | ||
| + | extern double distsharp_r; | ||
| + | double calculateSHARP(uint8_t side); | ||
| + | double measureSHARP(uint8_t side); | ||
| + | void setSHARPSPower(uint8_t pwr); | ||
| + | #endif | ||
| + | // SRF02 sensors: | ||
| + | #ifdef SRF02_SENSORS | ||
| + | extern uint16_t distsrf02; | ||
| + | uint8_t SRF02_i2cadr(uint8_t channel); | ||
| + | uint8_t SRF02_getFirmware(uint8_t channel); | ||
| + | void SRF02_ping(uint8_t channel, uint8_t mode); | ||
| + | uint16_t SRF02_read(uint8_t channel); | ||
| + | extern double distsrf02_l; | ||
| + | extern double distsrf02_r; | ||
| + | double SRF02_calculate(uint16_t us); | ||
| + | uint16_t SRF02_measure(uint8_t channel, uint8_t mode); | ||
| + | void SRF02_changeAdr(uint8_t adr, uint8_t newadr); | ||
| + | #endif | ||
| + | // Radar sensors: | ||
| + | #ifdef RADAR_SENSORS | ||
| + | void RADARS_init(void); | ||
| + | extern uint16_t adcradar_l; | ||
| + | extern uint16_t adcradar_r; | ||
| + | uint16_t getRADARSensor(uint8_t side); | ||
| + | extern double procradar_l; | ||
| + | extern double procradar_r; | ||
| + | double calculateRADAR(uint8_t side); | ||
| + | double measureRADAR(uint8_t side); | ||
| + | void setRADARSPower(uint8_t pwr); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // LFS and Bumper Board initialisation and shutdown: | ||
| + | |||
| + | void lfsbumper_init(void); | ||
| + | void lfsbumper_shutdown(void); | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #endif | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * | ||
| + | * ---> changes are documented in the file "RP6M256_LFSBumperLib.c" | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
| + | |||
| + | =====Library Source===== | ||
| + | Datei: RP6M256_LFSBumperLib.c: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 LFS Bumper Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * | ||
| + | * This is our new Library that contains basic routines and functions for | ||
| + | * accessing the components of the LFS and Bumper Board. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // LFS Board components: | ||
| + | // - 3 or 5 reflective optical sensors (CNY70) | ||
| + | |||
| + | // Bumper Board components: | ||
| + | // - 2 LEDs | ||
| + | // - 2 bumpers | ||
| + | // - 2 SHARP IR distance sensors (e.g. GP2Y0A02YK) | ||
| + | // - 2 SRF02 ultrasonic distance sensors (SRF02) | ||
| + | // - 2 Radar sensors (RSM-1650 based) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256_LFSBumperLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Variables: | ||
| + | |||
| + | uint8_t registerBuf[13]; | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Line Following Sensor (LFS) Board: | ||
| + | |||
| + | #ifdef LFS | ||
/** | /** | ||
* Call this once before using the LFS function. | * Call this once before using the LFS function. | ||
| Zeile 2.186: | Zeile 2.458: | ||
{ | { | ||
IO_MULTIIO_LFS_PWR_DDR |= IO_MULTIIO_LFS_PWR_IN; | IO_MULTIIO_LFS_PWR_DDR |= IO_MULTIIO_LFS_PWR_IN; | ||
| − | // LFS power | + | // LFS power on: |
IO_MULTIIO_LFS_PWR_PORT &= ~IO_MULTIIO_LFS_PWR_IN; | IO_MULTIIO_LFS_PWR_PORT &= ~IO_MULTIIO_LFS_PWR_IN; | ||
} | } | ||
| Zeile 2.242: | Zeile 2.514: | ||
IO_MULTIIO_LFS_PWR_PORT |= IO_MULTIIO_LFS_PWR_IN; | IO_MULTIIO_LFS_PWR_PORT |= IO_MULTIIO_LFS_PWR_IN; | ||
} | } | ||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
#endif | #endif | ||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // | + | // Bumper Board: |
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
// LEDs (always used): | // LEDs (always used): | ||
| − | |||
| − | |||
union { | union { | ||
uint8_t byte; | uint8_t byte; | ||
| Zeile 2.442: | Zeile 2.527: | ||
unsigned unused:6; | unsigned unused:6; | ||
}; | }; | ||
| − | } bumperLEDs; | + | } bumperLEDs; // Status of the bumper LEDs |
| + | |||
| + | /** | ||
| + | * Call this once before using the LEDs. | ||
| + | * | ||
| + | */ | ||
| + | void LEDS_init(void) | ||
| + | { | ||
| + | IO_MULTIIO_BUMPER_R_DDR |= IO_MULTIIO_BUMPER_R_IN; | ||
| + | IO_MULTIIO_BUMPER_R_PORT &= ~IO_MULTIIO_BUMPER_R_IN; | ||
| + | IO_MULTIIO_BUMPER_L_DDR |= IO_MULTIIO_BUMPER_L_IN; | ||
| + | IO_MULTIIO_BUMPER_L_PORT &= ~IO_MULTIIO_BUMPER_L_IN; | ||
| + | bumperLEDs.byte = 0; | ||
| + | } | ||
/** | /** | ||
| Zeile 2.454: | Zeile 2.552: | ||
void setbumperLEDL(uint8_t led) | void setbumperLEDL(uint8_t led) | ||
{ | { | ||
| − | |||
if (led > 0) { | if (led > 0) { | ||
IO_MULTIIO_BUMPER_L_PORT |= IO_MULTIIO_BUMPER_L_IN; | IO_MULTIIO_BUMPER_L_PORT |= IO_MULTIIO_BUMPER_L_IN; | ||
| Zeile 2.475: | Zeile 2.572: | ||
void setbumperLEDR(uint8_t led) | void setbumperLEDR(uint8_t led) | ||
{ | { | ||
| − | |||
if (led > 0) { | if (led > 0) { | ||
IO_MULTIIO_BUMPER_R_PORT |= IO_MULTIIO_BUMPER_R_IN; | IO_MULTIIO_BUMPER_R_PORT |= IO_MULTIIO_BUMPER_R_IN; | ||
| Zeile 2.511: | Zeile 2.607: | ||
} | } | ||
| − | |||
// Bumpers: | // Bumpers: | ||
| − | |||
#ifdef BUMPERS | #ifdef BUMPERS | ||
/** | /** | ||
| Zeile 2.523: | Zeile 2.617: | ||
* | * | ||
*/ | */ | ||
| − | void | + | void MULTIIO_BUMPERS_init(void) |
{ | { | ||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
startStopwatch8(); | startStopwatch8(); | ||
} | } | ||
| − | |||
| − | |||
| − | |||
/** | /** | ||
* This function reads and returns the value of | * This function reads and returns the value of | ||
| − | * the left or right bumper | + | * the left or right bumper. |
| − | + | ||
* | * | ||
| − | * Input: side -> | + | * Input: side -> 2 (LEFT) = Left bumper |
| − | * | + | * 3 (RIGHT) = Right bumper |
* | * | ||
*/ | */ | ||
| − | uint8_t | + | uint8_t getMULTIIO_BUMPER(uint8_t side) |
{ | { | ||
| + | uint8_t tmp; | ||
| + | |||
if (side == LEFT) { | if (side == LEFT) { | ||
IO_MULTIIO_BUMPER_L_PORT &= ~IO_MULTIIO_BUMPER_L_IN; | IO_MULTIIO_BUMPER_L_PORT &= ~IO_MULTIIO_BUMPER_L_IN; | ||
IO_MULTIIO_BUMPER_L_DDR &= ~IO_MULTIIO_BUMPER_L_IN; | IO_MULTIIO_BUMPER_L_DDR &= ~IO_MULTIIO_BUMPER_L_IN; | ||
| − | + | sleep(5); | |
if ((IO_MULTIIO_BUMPER_L_PIN & IO_MULTIIO_BUMPER_L_IN) > 0) | if ((IO_MULTIIO_BUMPER_L_PIN & IO_MULTIIO_BUMPER_L_IN) > 0) | ||
| − | + | tmp = 1; | |
| − | else | + | else tmp = 0; |
IO_MULTIIO_BUMPER_L_DDR |= IO_MULTIIO_BUMPER_L_IN; | IO_MULTIIO_BUMPER_L_DDR |= IO_MULTIIO_BUMPER_L_IN; | ||
if (bumperLEDs.LEDL) { | if (bumperLEDs.LEDL) { | ||
IO_MULTIIO_BUMPER_L_PORT |= IO_MULTIIO_BUMPER_L_IN; | IO_MULTIIO_BUMPER_L_PORT |= IO_MULTIIO_BUMPER_L_IN; | ||
} | } | ||
| − | return | + | return tmp; |
} | } | ||
if (side == RIGHT) { | if (side == RIGHT) { | ||
IO_MULTIIO_BUMPER_R_PORT &= ~IO_MULTIIO_BUMPER_R_IN; | IO_MULTIIO_BUMPER_R_PORT &= ~IO_MULTIIO_BUMPER_R_IN; | ||
IO_MULTIIO_BUMPER_R_DDR &= ~IO_MULTIIO_BUMPER_R_IN; | IO_MULTIIO_BUMPER_R_DDR &= ~IO_MULTIIO_BUMPER_R_IN; | ||
| − | + | sleep(5); | |
if ((IO_MULTIIO_BUMPER_R_PIN & IO_MULTIIO_BUMPER_R_IN) > 0) | if ((IO_MULTIIO_BUMPER_R_PIN & IO_MULTIIO_BUMPER_R_IN) > 0) | ||
| − | + | tmp = 1; | |
| − | else | + | else tmp = 0; |
IO_MULTIIO_BUMPER_R_DDR |= IO_MULTIIO_BUMPER_R_IN; | IO_MULTIIO_BUMPER_R_DDR |= IO_MULTIIO_BUMPER_R_IN; | ||
if (bumperLEDs.LEDR) { | if (bumperLEDs.LEDR) { | ||
IO_MULTIIO_BUMPER_R_PORT |= IO_MULTIIO_BUMPER_R_IN; | IO_MULTIIO_BUMPER_R_PORT |= IO_MULTIIO_BUMPER_R_IN; | ||
} | } | ||
| − | return | + | return tmp; |
} | } | ||
return 0; | return 0; | ||
| Zeile 2.577: | Zeile 2.664: | ||
// ------------------------------- | // ------------------------------- | ||
| − | // Bumpers State changed handler: | + | // MultiIO Bumpers State changed handler: |
| − | void | + | void MULTIIO_BUMPERS_stateChanged_DUMMY(void){} |
| − | static void (* | + | static void (*MULTIIO_BUMPERS_stateChangedHandler)(void) = MULTIIO_BUMPERS_stateChanged_DUMMY; |
/** | /** | ||
| − | * Use this function to set the Bumpers state change handler. | + | * Use this function to set the MultiIO Bumpers state change handler. |
* | * | ||
*/ | */ | ||
| − | void | + | void MULTIIO_BUMPERS_setStateChangedHandler(void (*bumperHandler)(void)) |
{ | { | ||
| − | + | MULTIIO_BUMPERS_stateChangedHandler = bumperHandler; | |
} | } | ||
// ------------------------------- | // ------------------------------- | ||
| + | |||
| + | uint8_t iobumper_l; | ||
| + | uint8_t iobumper_r; | ||
/** | /** | ||
| Zeile 2.597: | Zeile 2.687: | ||
* used everywhere in your program. It can also | * used everywhere in your program. It can also | ||
* call an event handler routine, that you need to | * call an event handler routine, that you need to | ||
| − | * register with | + | * register with MULTIIO_BUMPERS_setStateChangedHandler |
* before. | * before. | ||
* | * | ||
*/ | */ | ||
| − | void | + | void task_MULTIIO_BUMPERS(void) |
{ | { | ||
if(getStopwatch8() > 50) { // 50ms | if(getStopwatch8() > 50) { // 50ms | ||
| − | uint8_t left = | + | uint8_t left = getMULTIIO_BUMPER(LEFT); |
| − | uint8_t right = | + | uint8_t right = getMULTIIO_BUMPER(RIGHT); |
if(iobumper_l != left || iobumper_r != right) { | if(iobumper_l != left || iobumper_r != right) { | ||
iobumper_l = left; | iobumper_l = left; | ||
iobumper_r = right; | iobumper_r = right; | ||
| − | + | MULTIIO_BUMPERS_stateChangedHandler(); | |
} | } | ||
setStopwatch8(0); | setStopwatch8(0); | ||
| Zeile 2.616: | Zeile 2.706: | ||
#endif | #endif | ||
| − | |||
// SHARP sensors: | // SHARP sensors: | ||
| − | |||
#ifdef SHARP_SENSORS | #ifdef SHARP_SENSORS | ||
/** | /** | ||
| Zeile 2.639: | Zeile 2.727: | ||
* also stored in adcsharp_l or adcsharp_r. | * also stored in adcsharp_l or adcsharp_r. | ||
* | * | ||
| − | * Input: side -> | + | * Input: side -> 2 (LEFT) = Left SHARP sensor |
| − | * | + | * 3 (RIGHT) = Right SHARP sensor |
* | * | ||
*/ | */ | ||
| Zeile 2.665: | Zeile 2.753: | ||
* getSHARPSensor(). | * getSHARPSensor(). | ||
* | * | ||
| − | * Input: side -> | + | * Input: side -> 2 (LEFT) = Left SHARP sensor |
| − | * | + | * 3 (RIGHT) = Right SHARP sensor |
* | * | ||
* Hints: - The linearisation formula is adapted to | * Hints: - The linearisation formula is adapted to | ||
| Zeile 2.700: | Zeile 2.788: | ||
* stored in adcsharp_l or adcsharp_r. | * stored in adcsharp_l or adcsharp_r. | ||
* | * | ||
| − | * Input: side -> | + | * Input: side -> 2 (LEFT) = Left SHARP sensor |
| − | * | + | * 3 (RIGHT) = Right SHARP sensor |
* | * | ||
*/ | */ | ||
| Zeile 2.733: | Zeile 2.821: | ||
#endif | #endif | ||
| − | |||
// SRF02 sensors: | // SRF02 sensors: | ||
| − | |||
#ifdef SRF02_SENSORS | #ifdef SRF02_SENSORS | ||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
| − | |||
uint16_t distsrf02; // SRF02 distance [inch, cm, us] | uint16_t distsrf02; // SRF02 distance [inch, cm, us] | ||
| Zeile 2.806: | Zeile 2.836: | ||
* 6 = SRF02 sensor 4 (CH_SRF02_4) " | * 6 = SRF02 sensor 4 (CH_SRF02_4) " | ||
* *) Located on the Bumper Board! | * *) Located on the Bumper Board! | ||
| − | * ") Connected to the MultiIO | + | * ") Connected to the MultiIO Board! |
* | * | ||
*/ | */ | ||
| Zeile 2.910: | Zeile 2.940: | ||
double SRF02_calculate(uint16_t us) | double SRF02_calculate(uint16_t us) | ||
{ | { | ||
| − | + | double time_ms = us / 2000.0f; // One way [ms] | |
return (SONIC_SPEED * time_ms); // Distance [mm] | return (SONIC_SPEED * time_ms); // Distance [mm] | ||
} | } | ||
| Zeile 2.966: | Zeile 2.996: | ||
#endif | #endif | ||
| − | /**** | + | // Radar sensors: |
| − | // | + | #ifdef RADAR_SENSORS |
| + | /** | ||
| + | * Call this once before using the Radar sensors. | ||
| + | * | ||
| + | */ | ||
| + | void RADARS_init(void) | ||
| + | { | ||
| + | IO_MULTIIO_SHARPS_PWR_DDR |= IO_MULTIIO_SHARPS_PWR_IN; | ||
| + | // Radar sensors power on: | ||
| + | IO_MULTIIO_SHARPS_PWR_PORT &= ~IO_MULTIIO_SHARPS_PWR_IN; | ||
| + | } | ||
| + | |||
| + | uint16_t adcradar_l; // Left Radar sensor ADC value | ||
| + | uint16_t adcradar_r; // Right Radar sensor ADC value | ||
/** | /** | ||
| − | * | + | * This function reads and returns the ADC value |
| − | * if | + | * of the left or right Radar sensor. The value is |
| − | * | + | * also stored in adcradar_l or adcradar_r. |
| + | * | ||
| + | * Input: side -> 2 (LEFT) = Left Radar sensor | ||
| + | * 3 (RIGHT) = Right Radar sensor | ||
| + | * | ||
| + | */ | ||
| + | uint16_t getRADARSensor(uint8_t side) | ||
| + | { | ||
| + | if (side == LEFT) { | ||
| + | adcradar_l = readADC(ADC_MULTIIO_SHARP_L); | ||
| + | return adcradar_l; | ||
| + | } | ||
| + | if (side == RIGHT) { | ||
| + | adcradar_r = readADC(ADC_MULTIIO_SHARP_R); | ||
| + | return adcradar_r; | ||
| + | } | ||
| + | return 0; | ||
| + | } | ||
| + | |||
| + | double procradar_l; // Left Radar sensor result | ||
| + | double procradar_r; // Right Radar sensor result | ||
| + | |||
| + | /** | ||
| + | * Calculates and returns the left or right Radar | ||
| + | * sensor ADC result by using the data read from | ||
| + | * the Radar sensor with the function | ||
| + | * getRADARSensor(). | ||
| + | * | ||
| + | * Input: side -> 2 (LEFT) = Left Radar sensor | ||
| + | * 3 (RIGHT) = Right Radar sensor | ||
| + | * | ||
| + | * Hints: - !This function is not ready for use! | ||
| + | * - Please add your processing of the | ||
| + | * ADC results here! | ||
| + | * | ||
| + | */ | ||
| + | double calculateRADAR(uint8_t side) | ||
| + | { | ||
| + | double vside; | ||
| + | |||
| + | if (side == LEFT) { | ||
| + | vside = adcradar_l * 5.0 / 1024.0; | ||
| + | //vside = ... // Process the Radar sensor ADC result! | ||
| + | return vside; | ||
| + | } | ||
| + | if (side == RIGHT) { | ||
| + | vside = adcradar_r * 5.0 / 1024.0; | ||
| + | //vside = ... // Process the Radar sensor ADC result! | ||
| + | return vside; | ||
| + | } | ||
| + | return 0; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Measures and returns the processed left or | ||
| + | * right Radar sensor result. | ||
| + | * The ADC value of the Radar sensor is also | ||
| + | * stored in adcradar_l or adcradar_r. | ||
| + | * | ||
| + | * Input: side -> 2 (LEFT) = Left Radar sensor | ||
| + | * 3 (RIGHT) = Right Radar sensor | ||
| + | * | ||
| + | */ | ||
| + | double measureRADAR(uint8_t side) | ||
| + | { | ||
| + | uint16_t tmp; | ||
| + | |||
| + | tmp = getRADARSensor(side); | ||
| + | return (calculateRADAR(side)); // Result | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * With this function you can switch the Radar | ||
| + | * sensors power on or off. | ||
| + | * | ||
| + | * Input: pwr -> 0 (false) = Radar sensors | ||
| + | * power off | ||
| + | * >0 (true) = Radar sensors | ||
| + | * power on | ||
| + | * | ||
| + | * Hint: If the Radar sensors are not used, you | ||
| + | * should always switch the Radar sensors | ||
| + | * power off to save energy! | ||
| + | * | ||
| + | */ | ||
| + | void setRADARSPower(uint8_t pwr) | ||
| + | { | ||
| + | if (pwr > 0) | ||
| + | IO_MULTIIO_SHARPS_PWR_PORT &= ~IO_MULTIIO_SHARPS_PWR_IN; | ||
| + | else | ||
| + | IO_MULTIIO_SHARPS_PWR_PORT |= IO_MULTIIO_SHARPS_PWR_IN; | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | // Bumper Board init: | ||
| + | /** | ||
| + | * Call this once before using the Bumper Board. | ||
* | * | ||
*/ | */ | ||
void BUMPERBOARD_init(void) | void BUMPERBOARD_init(void) | ||
{ | { | ||
| + | LEDS_init(); | ||
#ifdef BUMPERS | #ifdef BUMPERS | ||
| − | + | MULTIIO_BUMPERS_init(); | |
#endif | #endif | ||
#ifdef SHARP_SENSORS | #ifdef SHARP_SENSORS | ||
SHARPS_init(); | SHARPS_init(); | ||
| + | #endif | ||
| + | #ifdef RADAR_SENSORS | ||
| + | RADARS_init(); | ||
#endif | #endif | ||
} | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // LFS and Bumper Board initialisation and shutdown: | ||
| + | |||
| + | /** | ||
| + | * You MUST call this function at the beginning of a | ||
| + | * main program, that uses the LFS or Bumper Board. | ||
| + | * | ||
| + | */ | ||
| + | void lfsbumper_init(void) | ||
| + | { | ||
| + | // LFS: | ||
| + | #ifdef LFS | ||
| + | LFS_init(); // Init LFS Board | ||
| + | #endif | ||
| + | // Bumper: | ||
| + | BUMPERBOARD_init(); // Init Bumper Board | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * If you don't use any function of the LFS and | ||
| + | * Bumper Board, you can put the LFS and Bumper | ||
| + | * Board into "SHUTDOWN MODE". In this mode the | ||
| + | * power consumption is very low to save energy. | ||
| + | * | ||
| + | */ | ||
| + | void lfsbumper_shutdown(void) | ||
| + | { | ||
| + | // LEDs: | ||
| + | setbumperLEDs(0b00); // Bumper LEDs off | ||
| + | // Bumper: | ||
| + | #ifdef SHARP_SENSORS | ||
| + | setSHARPSPower(0); // SHARPS/RADARS power off | ||
| + | #endif | ||
| + | #ifdef RADAR_SENSORS | ||
| + | setRADARSPower(0); | ||
| + | #endif | ||
| + | // LFS: | ||
| + | #ifdef LFS | ||
| + | setLFSPower(0); // LFS power off | ||
| + | #endif | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * - v. 1.2 (function SRF02_calculate bug fixed) 18.10.2014 by Dirk | ||
| + | * - v. 1.1 (adapted for use with I2CMasterLib) 09.05.2013 by Dirk | ||
| + | * - v. 1.0 (initial release) 28.03.2013 by Dirk | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
| + | |||
| + | ======Erklärung====== | ||
| + | |||
| + | =====Linienfolger Demo===== | ||
| + | Fast jeder mobile Roboter hat inzwischen Linienfolge-Sensoren (LFS). Der RP6 bekommt sie mit der Multi IO Projekt Platine jetzt auch. Das Liniensensor Board muss dazu an den 8-poligen Stecker "CNY70" auf der MultiIO angeschlossen und vorn am RP6 knapp über dem Boden montiert werden. | ||
| + | |||
| + | Gegenüber der Standardstellung der Jumper (siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier!]]) müssen Jumper auf der MultiIO für die LFS-Nutzung umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block. | ||
| + | |||
| + | '''Wahl-Jumper: Stellung der Jumper''' | ||
| + | |||
| + | [[Bild:MultiIO_Wahl-Jumper_2.PNG|LFS Jumper Stellung]] | ||
| + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | ||
| + | |||
| + | Zur Messung der 3,3V-Spannung und zur Auswertung des | ||
| + | Touch-Sensors kann zusätzlich je ein Jumper auf | ||
| + | '''3V3-PF4''' und '''NE5-PF3''' gesetzt werden, wenn keine | ||
| + | analogen Sensoren auf der Bumper-Platine benutzt | ||
| + | werden. | ||
| + | |||
| + | makefile: | ||
| + | <pre>... | ||
| + | TARGET = RP6M256_MultiIO_2 | ||
| + | ... | ||
| + | SRC += RP6M256_LFSBumperLib.c | ||
| + | ...</pre> | ||
| + | |||
| + | Datei: RP6M256_MultiIO_2.c: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 MultiIO | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * In this example we show a second test for the MultiIO Project Board. | ||
| + | * It tests the Line Following Sensor (LFS) Board with 5 (3) CNY70s. | ||
| + | * | ||
| + | * ############################################################################ | ||
| + | * The Robot does NOT move in this example! You can simply put it on a table | ||
| + | * next to your PC and you should connect it to the PC via the USB Interface! | ||
| + | * You should also connect to it via WIFI. | ||
| + | * ############################################################################ | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" // The RP6 M256 Library. | ||
| + | // Always needs to be included! | ||
| + | #include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Include our new "RP6M256 LFS Bumper library": | ||
| + | // (This is the library for accessing the LFS and Bumper Board!) | ||
| + | |||
| + | #include "RP6M256_LFSBumperLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /** | ||
| + | * Write a floating point number to the WIFI. | ||
| + | * | ||
| + | * Example: | ||
| + | * | ||
| + | * // Write a floating point number to the WIFI (no exponent): | ||
| + | * writeDouble_WIFI(1234567.890, 11, 3); | ||
| + | * | ||
| + | * The value of prec (precision) defines the number of decimal places. | ||
| + | * For 32 bit floating point variables (float, double ...) 6 is | ||
| + | * the max. value for prec (7 relevant digits). | ||
| + | * The value of width defines the overall number of characters in the | ||
| + | * floating point number including the decimal point. The number of | ||
| + | * pre-decimal positions is: (width - prec - 1). | ||
| + | */ | ||
| + | void writeDouble_WIFI(double number, uint8_t width, uint8_t prec) | ||
| + | {char buffer[width + 1]; | ||
| + | dtostrf(number, width, prec, &buffer[0]); | ||
| + | writeString_WIFI(&buffer[0]); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Error handler | ||
| + | |||
| + | /** | ||
| + | * This function gets called automatically if there was an I2C Error like | ||
| + | * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30). | ||
| + | * | ||
| + | */ | ||
| + | void I2C_transmissionError(uint8_t errorState) | ||
| + | { | ||
| + | writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x"); | ||
| + | writeInteger_WIFI(errorState, HEX); | ||
| + | writeChar_WIFI('\n'); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Main function - The program starts here: | ||
| + | |||
| + | int main(void) | ||
| + | { | ||
| + | initRP6M256(); // Always call this first! The Processor will not work | ||
| + | // correctly otherwise. | ||
| + | |||
| + | initLCD(); // Initialize the LC-Display (LCD) | ||
| + | // Always call this before using the LCD! | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | mSleep(500); | ||
| + | setLEDs(0b0000); | ||
| + | |||
| + | writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 2!\n"); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation | ||
| + | // with 100kHz SCL Frequency | ||
| + | |||
| + | |||
| + | // Register the event handler: | ||
| + | I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | |||
| + | // Write a text message to the LCD: | ||
| + | showScreenLCD("################", "################"); | ||
| + | mSleep(1500); | ||
| + | showScreenLCD("RP6v2-M256-WIFI ", "Example Program"); | ||
| + | mSleep(2500); | ||
| + | showScreenLCD("RP6M256 Multi IO", " Selftest 2"); | ||
| + | mSleep(2500); | ||
| + | clearLCD(); | ||
| + | |||
| + | setLEDs(0b0000); | ||
| + | |||
| + | // --------------------------------------- | ||
| + | |||
| + | uint16_t lfs_l, lfs_m, lfs_r; | ||
| + | #ifdef FIVE_SENSORS | ||
| + | uint16_t lfs_l1, lfs_r1; | ||
| + | #endif | ||
| + | |||
| + | startStopwatch1(); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | lfsbumper_init(); // LFS & Bumper init!!! | ||
| + | //setServoPower(1); // Servo power ON! | ||
| + | |||
| + | while(true) | ||
| + | { | ||
| + | if(getStopwatch1() > 500) // 0.5s | ||
| + | { | ||
| + | // LFS ADC test: | ||
| + | #ifdef FIVE_SENSORS | ||
| + | lfs_l1 = getLFS(CH_LFS_L1); | ||
| + | writeString_WIFI("\nADC LFS_L1: "); | ||
| + | writeInteger_WIFI(lfs_l1, DEC); | ||
| + | #endif | ||
| + | lfs_l = getLFS(CH_LFS_L); | ||
| + | writeString_WIFI("\nADC LFS_L: "); | ||
| + | writeInteger_WIFI(lfs_l, DEC); | ||
| + | lfs_m = getLFS(CH_LFS_M); | ||
| + | writeString_WIFI("\nADC LFS_M: "); | ||
| + | writeInteger_WIFI(lfs_m, DEC); | ||
| + | lfs_r = getLFS(CH_LFS_R); | ||
| + | writeString_WIFI("\nADC LFS_R: "); | ||
| + | writeInteger_WIFI(lfs_r, DEC); | ||
| + | #ifdef FIVE_SENSORS | ||
| + | lfs_r1 = getLFS(CH_LFS_R1); | ||
| + | writeString_WIFI("\nADC LFS_R1: "); | ||
| + | writeInteger_WIFI(lfs_r1, DEC); | ||
| + | #endif | ||
| + | writeString_WIFI("\n"); | ||
| + | |||
| + | setStopwatch1(0); | ||
| + | } | ||
| + | |||
| + | // task_I2CTWI(); | ||
| + | } | ||
| + | |||
| + | return 0; | ||
| + | } | ||
| + | </pre> | ||
| + | |||
| + | ======Erklärung====== | ||
| + | |||
| + | =====Bumper Demo===== | ||
| + | Fabian hat ein sehr universelles Bumper-Board designt, auf das 2 LEDs, 2 Bumper-Taster, zusätzlich 2 analoge SHARP-IR-Entfernungssensoren (in dieser Demo: GP2Y0A02YK), alternativ 2 I2C-Ultraschall-Entfernungssensoren (SRF02) oder 2 Radar-Sensoren (mit RSM-1650) montiert werden können. Das Bumper-Board muss an den 7-poligen Stecker "Bumper" der MultiIO angeschlossen und vorn oder hinten am RP6 montiert werden. | ||
| + | |||
| + | Diese Software demonstriert das Auslesen aller dieser Sensoren bis auf die Radar-Sensoren (weil ich die Radar-Sensoren noch nicht bekommen habe!). | ||
| + | |||
| + | Gegenüber der Standardstellung der Jumper (siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier!]]) müssen Jumper auf der MultiIO für die Nutzung des Bumper-Boards umgesteckt werden. Die folgenden Abbildungen zeigen den Wahl-Jumper-Block. | ||
| + | |||
| + | '''Wahl-Jumper: Stellung der Jumper für analoge Sensoren (SHARP, Radar)''' | ||
| + | |||
| + | [[Bild:MultiIO_Wahl-Jumper_2a.PNG|Bumper analog Jumper Stellung]] | ||
| + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | ||
| + | Diese Jumperstellung entspricht der Standard-Jumperstellung! | ||
| + | |||
| + | '''Wahl-Jumper: Stellung der Jumper für I2C-Sensoren (SRF02)''' | ||
| + | |||
| + | [[Bild:MultiIO_Wahl-Jumper_2b.PNG|Bumper I2C Jumper Stellung]] | ||
| + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | ||
| + | Zur Messung der 3,3V-Spannung und zur Auswertung des | ||
| + | Touch-Sensors kann zusätzlich je ein Jumper auf '''3V3-PF4''' | ||
| + | und '''NE5-PF3''' gesetzt werden. | ||
| + | Alternativ kann diese Messung auch wie bei der Standard- | ||
| + | Jumperstellung mit Jumpern auf '''3V3-AD3''' und '''NE5-AD1''' | ||
| + | erfolgen, wenn man die LFS-Kanäle L und M nicht braucht. | ||
| + | |||
| + | makefile: | ||
| + | <pre>... | ||
| + | TARGET = RP6M256_MultiIO_3 | ||
| + | ... | ||
| + | SRC += RP6M256_LFSBumperLib.c | ||
| + | ...</pre> | ||
| + | |||
| + | Datei RP6M256_MultiIO_3.c: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 MultiIO | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * In this example we show a third test for the MultiIO Project Board. | ||
| + | * It tests the Bumper Board with 2 LEDs, 2 bumpers, 2 analog SHARP IR | ||
| + | * distance sensors (GP2Y0A02YK) and/or 2 I2C ultrasonic distance sensors | ||
| + | * (SRF02) and/or 2 Radar sensors (RSM-1650 based, see here: http://www. | ||
| + | * jokesoft.de/weidmann/index.php?option=com_content&view=section&layout= | ||
| + | * blog&id=17&Itemid=21). | ||
| + | * | ||
| + | * ############################################################################ | ||
| + | * The Robot does NOT move in this example! You can simply put it on a table | ||
| + | * next to your PC and you should connect it to the PC via the USB Interface! | ||
| + | * You should also connect to it via WIFI. | ||
| + | * ############################################################################ | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" // The RP6 M256 Library. | ||
| + | // Always needs to be included! | ||
| + | #include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Include our new "RP6M256 LFS Bumper library": | ||
| + | // (This is the library for accessing the LFS and Bumper Board!) | ||
| + | |||
| + | #include "RP6M256_LFSBumperLib.h" | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| Zeile 3.063: | Zeile 3.522: | ||
// --------------------------------------- | // --------------------------------------- | ||
| − | uint8_t bump_l, bump_r; | + | uint8_t i, bump_l, bump_r; |
uint16_t distsrf_l, distsrf_r; | uint16_t distsrf_l, distsrf_r; | ||
| Zeile 3.069: | Zeile 3.528: | ||
// IMPORTANT: | // IMPORTANT: | ||
| − | + | lfsbumper_init(); // LFS & Bumper init!!! | |
//setServoPower(1); // Servo power ON! | //setServoPower(1); // Servo power ON! | ||
| − | + | // Bumper LEDs test: | |
| + | for (i = 0; i < 10; i++) { | ||
| + | setbumperLEDL(1); // Left bumper LED on | ||
| + | setbumperLEDR(0); // Right bumper LED off | ||
| + | setLEDs(0b0101); | ||
| + | mSleep(500); | ||
| + | setbumperLEDs(0b10); // Right LED on, left LED off | ||
| + | setLEDs(0b1010); | ||
| + | mSleep(500); | ||
| + | } | ||
| + | setbumperLEDR(0); // Right bumper LED off | ||
| + | setLEDs(0b0000); | ||
while(true) | while(true) | ||
| Zeile 3.081: | Zeile 3.551: | ||
// Bumpers test: | // Bumpers test: | ||
clearLCD(); | clearLCD(); | ||
| − | bump_l = | + | bump_l = getMULTIIO_BUMPER(LEFT); // Read bumper |
| − | + | ||
setCursorPosLCD(0, 0); | setCursorPosLCD(0, 0); | ||
writeIntegerLCD(bump_l, DEC); | writeIntegerLCD(bump_l, DEC); | ||
setCursorPosLCD(0, 4); | setCursorPosLCD(0, 4); | ||
writeStringLCD("<BUMPER>"); | writeStringLCD("<BUMPER>"); | ||
| − | bump_r = | + | bump_r = getMULTIIO_BUMPER(RIGHT); |
setCursorPosLCD(0, 15); | setCursorPosLCD(0, 15); | ||
writeIntegerLCD(bump_r, DEC); | writeIntegerLCD(bump_r, DEC); | ||
| Zeile 3.107: | Zeile 3.576: | ||
writeString_WIFI("\nSRF02 SENSORS ->"); | writeString_WIFI("\nSRF02 SENSORS ->"); | ||
writeString_WIFI("\nLeft Distance: "); | writeString_WIFI("\nLeft Distance: "); | ||
| − | + | writeInteger_WIFI(distsrf_l, DEC); | |
distsrf_r = SRF02_measure(CH_SRF02_R, MODE_CM); | distsrf_r = SRF02_measure(CH_SRF02_R, MODE_CM); | ||
writeString_WIFI("cm\nRight Distance: "); | writeString_WIFI("cm\nRight Distance: "); | ||
| − | + | writeInteger_WIFI(distsrf_r, DEC); | |
writeString_WIFI("cm\n"); | writeString_WIFI("cm\n"); | ||
| + | #endif | ||
| + | #ifdef RADAR_SENSORS | ||
| + | // Radar sensors test: | ||
| + | procradar_l = measureRADAR(LEFT); // Measure | ||
| + | writeString_WIFI("\nRADAR SENSORS ->"); | ||
| + | writeString_WIFI("\nLeft Result: "); | ||
| + | writeDouble_WIFI(procradar_l, 5, 1); | ||
| + | writeString_WIFI("cm\nADC left Value: "); | ||
| + | writeInteger_WIFI(adcradar_l, DEC); | ||
| + | procradar_r = measureRADAR(RIGHT); | ||
| + | writeString_WIFI("\nRight Result: "); | ||
| + | writeDouble_WIFI(procradar_r, 5, 1); | ||
| + | writeString_WIFI("cm\nADC right Value: "); | ||
| + | writeInteger_WIFI(adcradar_r, DEC); | ||
| + | writeString_WIFI("\n"); | ||
#endif | #endif | ||
setStopwatch1(0); | setStopwatch1(0); | ||
| Zeile 3.125: | Zeile 3.609: | ||
======Erklärung====== | ======Erklärung====== | ||
| − | ====Radio Board | + | ====Radio Board Library==== |
Wird bei Bedarf DEN Usern zur Verfügung gestellt, die die | Wird bei Bedarf DEN Usern zur Verfügung gestellt, die die | ||
Radio-Platine bestellt haben und mit ihr arbeiten möchten. | Radio-Platine bestellt haben und mit ihr arbeiten möchten. | ||
| + | |||
| + | =====3,3V-Stromversorgung===== | ||
| + | Der I2C-AM/FM/SW/LW-Empfänger Si4735 wird mit einer Versorgungsspannung von 3,3V und an einem 3,3V-I2C-Bus betrieben. Diese Voraussetzungen sind gegeben, wenn der Empfänger an einen der beiden 5-poligen Anschluß-Stecker "3V3-I2C" auf der MultiIO angeschlossen wird. Allerdings muss noch die 3,3V-Stromversorgung der MultiIO eingeschaltet werden. Dazu sind zwei zusätzliche Jumper zu setzen, siehe folgende Abbildung: | ||
| + | |||
| + | [[Bild:MultiIO_J_3V3-Stromversorgung.PNG|Jumper der 3,3V-Stromversorgung]] | ||
| + | |||
| + | =====Library Header===== | ||
| + | =====Library Source===== | ||
| + | ======Erklärung====== | ||
| + | =====Demo===== | ||
| + | ======Erklärung====== | ||
====Library für die Orientierung im Raum==== | ====Library für die Orientierung im Raum==== | ||
| + | "Orientierung im Raum" ist ein vielschichtiger Begriff. Für einen mobilen Roboter ist es wichtig, seine Neigung (Pitch, Roll) zu kennen und ggf. seine Fahrtrichtung. In geschlossenen Räumen sollte er sich ebenfalls orientieren können, Hindernisse erkennen und z.B. eine Karte seiner Umgebung erstellen können. | ||
| + | |||
| + | In dieser Library soll es um das Auslesen von drei Sensoren gehen, die dabei helfen, sich zu orientieren: | ||
| + | * GPS-Empfänger (NL-552ETTL) | ||
| + | * 2D-Kompass (HDMM01) | ||
| + | * 9D-IMU (MinIMU-9 v2) | ||
| + | |||
| + | =====Achsen der Kompasssensoren===== | ||
| + | Die Library für die Orientierung im Raum geht davon aus, dass der Kompasssensor HDMM01 und die minIMU-9 v2 in einer bestimmten Ausrichtung auf der MultiIO bzw. auf dem RP6 befestigt sind. | ||
| + | Folgende Abbildung zeigt diese Anordnung: | ||
| + | |||
| + | [[Bild:MultiIO_Kompasssensoren.png|Achsen der Kompasssensoren]] | ||
| + | |||
| + | '''Wichtige Hinweise''' | ||
| + | * Zur Befestigung der Kompasssensoren sollte man keine ferromagnetischen Teile benutzen. | ||
| + | * Diese Sensoren müssen so angebracht werden, dass es keine Achsenabweichung zum RP6 gibt. Das heißt: Die Sensor-Platinen müssen genau parallel zur Fläche der Hauptplatine des RP6 ausgerichtet sein und ihre Kanten parallel zu den Kanten der RP6 Hauptplatine. | ||
| + | * Der Abstand der Kompasssensoren zur MultiIO Platine (oder zu anderen Teilen des RP6) sollte mehr als 5 cm betragen. | ||
| + | |||
| + | |||
| + | Diese [[Avr-gcc]] Library für das Multi IO Projekt Board (= "MultiIO") geht von folgenden Voraussetzungen aus: | ||
| + | * Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt. | ||
| + | * Die M256 ist der I2C-Bus Master. | ||
| + | * Die I2C-Bus Geschwindigkeit beträgt 100 kHz. | ||
| + | * Alle Jumper auf der MultiIO sind in ihrer Standardstellung (1). | ||
| + | * Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden. | ||
| + | * Der Wannenstecker IO_Mxxx der MultiIO ist mit dem Wannenstecker IO_PWM/T2/T3 der M256 1:1 verbunden. | ||
| − | + | Zu (1): Siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]! | |
=====UART-Anschluß===== | =====UART-Anschluß===== | ||
| Zeile 3.146: | Zeile 3.667: | ||
'''2. Man verbindet mit einem 2-poligen Kabel die beiden Pins "Tx" und "Rx5V" des 3-poligen Steckers "P_GPS1" direkt oberhalb des GPS-Modul-Anschlusses auf der MultiIO so mit dem Wannenstecker UART_SPI1/T5 der M256:''' | '''2. Man verbindet mit einem 2-poligen Kabel die beiden Pins "Tx" und "Rx5V" des 3-poligen Steckers "P_GPS1" direkt oberhalb des GPS-Modul-Anschlusses auf der MultiIO so mit dem Wannenstecker UART_SPI1/T5 der M256:''' | ||
| + | |||
| + | [[Bild:M256_UART1-GPS.PNG|M256: GPS-UART1-Verbindung]] | ||
| + | |||
{| {{Blauetabelle}} | {| {{Blauetabelle}} | ||
|- | |- | ||
| Zeile 3.161: | Zeile 3.685: | ||
'''WICHTIGER HINWEIS:''' | '''WICHTIGER HINWEIS:''' | ||
| − | '''Wenn die UART1-Verbindung zwischen GPS-Modul und M256 über den 3-poligen Stecker "P_GPS1" hergestellt wird, sollten die Tx- und Rx5V-Leitungen des GPS-Moduls von IO-Mxxx abgetrennt werden. Dazu müssen zwei Jumper der Standard-Jumperstellung (siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]!) an Pins 6 und 8 | + | '''Wenn die UART1-Verbindung zwischen GPS-Modul und M256 über den 3-poligen Stecker "P_GPS1" hergestellt wird, sollten die Tx- und Rx5V-Leitungen des GPS-Moduls von IO-Mxxx abgetrennt werden. Dazu müssen die zwei Jumper "Tx2" und "Rx" der Standard-Jumperstellung (siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]!) an Pins 6 und 8 vom Jumper-Block "J_IO" abgezogen werden. Siehe folgende Abbildung:''' |
[[Bild:MultiIO_J_IO-Mxxx_2.PNG|J_IO-Mxxx: UART-Abtrennung]] | [[Bild:MultiIO_J_IO-Mxxx_2.PNG|J_IO-Mxxx: UART-Abtrennung]] | ||
| + | ''Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!'' | ||
| + | |||
| + | |||
| + | Die '''Library''' (Software-Bibliothek) besteht aus drei Teilen: | ||
| + | * Dem [[RP6_Multi_IO_Projekt_-_Software#Configuration Header_2|Configuration Header]] -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der Orientation Library dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | ||
| + | * Dem [[RP6_Multi_IO_Projekt_-_Software#Library Header_4|Library Header]] -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library. | ||
| + | * Der [[RP6_Multi_IO_Projekt_-_Software#Library Source_4|Library Source]] -> Das ist die eigentliche Library. | ||
| + | |||
| + | Eingebunden wird auch die [[RP6v2_USB-RS232-Adapter#RP6M256uart1_Library|RP6M256uart1 Library]], mit der ein Auslesen des GPS-Moduls über UART1 der M256 möglich ist, und der [[RP6_Multi_IO_Projekt_-_Software#Configuration Header|MultiIO Configuration Header]]. '''Die RP6M256uart1_Library gehört in das Verzeichnis RP6Lib/RP6control_M256_WIFI'''. | ||
| + | |||
| + | =====Configuration Header===== | ||
| + | Vor dem Kompilieren der Library muss eine Anpassung des Configuration Headers an die eigenen Hardware-Voraussetzungen erfolgen. | ||
| + | Dazu gibt es im Configuration Header drei Definitionen: | ||
| + | * GPS -> Das GPS-Modul (NL-552ETTL) wird benutzt | ||
| + | * COMPASS_2D -> Der 2D-Compass (HDMM01) wird benutzt | ||
| + | * IMU_9D -> Die 9D-IMU (MinIMU-9 v2) wird verwendet | ||
| + | Im nachfolgenden Listing ist nur die Definition IMU_9D aktiv. Dies muss an die eigene Hardware angepaßt werden. Wenn z.B. die IMU NICHT benutzt werden soll, muss die Zeile mit "IMU_9D" auskommentiert werden. Dazu setzt man "//" an den Zeilenanfang. Beispiel: | ||
| + | //#define IMU_9D // 9D-IMU (MinIMU-9 v2) is used | ||
| + | |||
| + | Wenn der '''2D-Compass (HDMM01)''' benutzt wird, muss man zunächst den Sensor '''kalibrieren'''. Dazu müssen die Minimal- und Maximalwerte der beiden Sensor-Achsen ermittelt und in die Definitionen MAX_X_2DM, MIN_X_2DM, MAX_Y_2DM, MIN_Y_2DM eingetragen werden. | ||
| + | Dazu geht man so vor: | ||
| + | * Den RP6 mit montiertem HDMM01 auf eine waagerechte Fläche stellen,- am besten genau dort, wo er später herumfahren und sich mithilfe des Kompasses orientieren soll. Es dürfen keine Elektrogeräte, Metallteile und stromführende Leitungen in der Nähe sein, die das Magnetfeld verändern können. | ||
| + | * Die Demo starten und die X-Achsen und Y-Achsen Ausgaben im Wifi-Terminal oder auf dem LCD beobachten. | ||
| + | * Den RP6 langsam um die eigene (Z-)Achse drehen und dabei die Minimal- und Maximalwerte der X- und Y-Achse ermitteln. | ||
| + | * Die 4 Werte in den Configuration Header eintragen. | ||
| + | * Die Demo neu kompilieren. | ||
| + | |||
| + | Wenn die '''9D-IMU (MinIMU-9 v2)''' benutzt wird, muss man alle drei Komponenten (Gyrosensor, Beschleunigungssensor, Magnetfeldsensor) der IMU '''kalibrieren'''. Dazu geht man so vor: | ||
| + | * '''Gyrosensor:''' | ||
| + | ** Wenn der RP6 ruhig auf einer ebenen Fläche steht und nicht bewegt wird, sollten die Werte der X-, Y- und Z-Achse um den Nullpunkt schwanken. Tatsächlich wird es Abweichungen zum Nullwert geben. Zur Kompensierung der Null-Abweichung kann man die Definitionen OFFSET_X, OFFSET_Y und OFFSET_Z verwenden, um die Achsen zu "nullen". Zeigt die X-Achse in Ruhe z.B. 756 an, trägt man für OFFSET_X -756 ein. | ||
| + | ** Da die Werte der 3 Achsen begrenzt werden, muss man die Minimal- und Maximalwerte der X-, Y- und Z-Achse festlegen und in die Definitionen MAX_X, MIN_X, MAX_Y, MIN_Y eintragen. Für den Anfang kann man die Werte bei +-20000 belassen. | ||
| + | ** Wenn man den Temperaturwert des L3GD20 nutzen will, muss man die Definition GET_TEMP aktivieren (Standard!). Den Temperaturwert kann man noch eichen, indem man die Definition OFFSET_TEMP anpasst (ganzzahliger Wert!). Die Einheit von OFFSET_TEMP ist °C. | ||
| + | * '''Beschleunigungssensor:''' | ||
| + | ** Die X-Achse wird kalibriert, indem man den RP6 mit der rechten Seite nach unten hält und den Maximalwert in MAX_X_A einträgt. Dann neigt man ihn auf seine linke Seite und trägt den negativen Maximalwert in MIN_X_A ein. | ||
| + | ** Die Y-Achse wird kalibriert, indem man den RP6 mit der Vorderseite nach oben hält und den Maximalwert in MAX_Y_A einträgt. Dann kippt man die Vorderseite nach unten und trägt den negativen Maximalwert in MIN_Y_A ein. | ||
| + | ** Die Z-Achse wird kalibriert, indem man den RP6 normal waagerecht hält und den Maximalwert in MAX_Z_A einträgt. Dann dreht man den RP6 "auf den Kopf" und trägt den negativen Maximalwert in MIN_Z_A ein. | ||
| + | ** Sollten die angezeigten Pitch- und Roll-Werte nach der Kalibrierung der Achsen und genau waagerecht stehendem Sensor bzw. RP6 vom Nullwert deutlich abweichen, kann man sie mithilfe der Definitionen OFFSET_PITCH_A und OFFSET_ROLL_A "nullen". Die Einheit dieser Definitionen ist °. | ||
| + | ''HINWEIS: Die internen Berechnungen werden durch diese Offset-Korrektur | ||
| + | NICHT beeinflußt. Die Offset-Werte können also nicht dazu benutzt werden, | ||
| + | eine Achsenabweichung zwischen Sensor und RP6 zu kompensieren!'' | ||
| + | * '''Magnetfeldsensor:''' | ||
| + | ** Zur Kalibrierung des Magnetfeldsensors stellt man den RP6 auf eine waagerechte Fläche. Es dürfen keine Elektrogeräte, Metallteile und stromführende Leitungen in der Nähe sein, die das Magnetfeld verändern können. | ||
| + | ** Zunächst ist es möglich, die sogenannten "hard iron" Effekte auszugleichen. Dies sind feste Magnetfeldabweichungen, die durch das "Gerät" (in unserem Fall also durch den kompletten RP6) verursacht werden, in oder auf dem der Sensor angebracht ist. Mit den Definitionen OFFSET_X_M, OFFSET_Y_M und OFFSET_Z_M können diese Effekte ausgeglichen werden. Diese Kalibrierung wird hier aktuell (noch) nicht beschrieben, so dass man die Werte für diese Definitionen zunächst bei 0 belassen sollte. | ||
| + | ** Die X-Achse und Y-Achse wird kalibriert, indem man den RP6 langsam mehrfach um die eigene (Z-)Achse dreht und die Maximalwerte in MAX_X_M und MAX_Y_M einträgt. Die negativen Maximalwerte trägt man in MIN_X_M und MIN_Y_M ein. | ||
| + | ** Die Z-Achse wird kalibriert, indem man den RP6 auf die rechte oder linke Seite legt, ihn dann langsam mehrfach auf der Stelle (um seine Y-Achse) dreht und den Maximalwert in MAX_Z_M und den negativen Maximalwert in MIN_Z_M einträgt. | ||
| + | ** Mit der Definition DECLINATION_M kann man zur Verbesserung der Genauigkeit der Richtungsangaben den magnetischen Deklinationswert [°] (d.h. die magnetische Missweisung) am aktuellen Standort festlegen. [http://www.ngdc.noaa.gov/geomag-web/#declination Hier] befindet sich ein "Deklinations-Rechner", mit dem man den Deklinationswert für den eigenen Standort ermitteln kann. | ||
| + | ** Wenn man den Temperaturwert des LSM303DLHC nutzen will, muss man die Definition GET_TEMP_M aktivieren (Standard!). Den Temperaturwert kann man noch eichen, indem man die Definition OFFSET_TEMP_M anpasst. Die Einheit von OFFSET_TEMP_M ist °C. | ||
| + | |||
| + | '''HINWEISE ZUR KALIBRIERUNG:''' | ||
| + | * Das Demo-Programm ist nicht sehr gut geeignet, um die Kalibrierungen durchzuführen. | ||
| + | * Man sollte mit einem eigenen kurzen ''Kalibrierungs-Programm'' nur die Rohwerte der jeweiligen X-, Y- und Z-Achse z.B. zweimal pro Sekunde ausgeben. | ||
| + | * Die Rohwerte sind in Variablen x_axis..., y_axis..., z_axis... enthalten. Da die Rohwerte in den Funktionen normalizeHDMM01(), normalizeL3GD20(), normalizeLSM303DLHC_A() bzw. normalizeLSM303DLHC_M() ggf. verändert werden, müssen die Rohwerte VOR Durchlaufen dieser Funktionen ausgegeben werden. | ||
| + | * '''Ohne eine sorgfältige Kalibrierung wird die Kompass-Funktion (insbesondere die neigungskompensierte Ausgabe der Himmelsrichtung) nicht funktionieren!''' | ||
| + | * Nach dem Eintragen der Kalibrierungs-Werte in den Configuration Header (RP6M256_Orientation.h) muss die ''Demo'' neu kompiliert werden. Vorher sollten die Compiler-Hilfsdateien der Orientierungs-Library (Dateiendungen .st und .o) im Programmverzeichnis gelöscht werden. | ||
| + | * Getestet wird das Ergebnis der Kalibrierung, indem die Ausgabewerte (Temperaturen, Pitch, Roll, Heading, Neigungs-kompensiertes Heading) der ''Demo'' überprüft werden. Dazu kann man Vergleichswerte (Thermometer, Winkelmesser für Pitch, Roll und externer Kompass für Heading) heranziehen. | ||
| + | * Wenn die Ausgabewerte nicht den Erwartungen entsprechen, muss die Kalibrierung wiederholt werden. | ||
| + | * Eine Kalibrierung der Sensoren ist nur zuverlässig, solange die Position des jeweiligen Sensors und seine komplette Hardware-Umgebung (also der RP6 mit allen seinen Teilen!) nicht verändert wird. Gibt es trotzdem Veränderungen, z.B. durch weitere Aufbauten auf dem RP6, müssen diese Sensoren neu kalibriert werden. | ||
| + | |||
| + | |||
| + | Datei: RP6M256_Orientation.h: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 Orientation Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * Configuration header file for new Orientation library. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | #ifndef RP6M256_ORIENTATION_H | ||
| + | #define RP6M256_ORIENTATION_H | ||
| + | |||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // The following sensors are used: | ||
| + | // - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS | ||
| + | // chipset | ||
| + | // - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG | ||
| + | // 2-axis magnetometer | ||
| + | // - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and | ||
| + | // the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | // Define the devices that are connected to the MultiIO Project Board here! | ||
| + | // Make a comment of a line belonging to a sensor that is NOT attached to | ||
| + | // the MultiIO Board! | ||
| + | //#define GPS // GPS module (NL-552ETTL) is used | ||
| + | //#define COMPASS_2D // 2D-Compass module (HDMM01) is used | ||
| + | #define IMU_9D // 9D-IMU (MinIMU-9 v2) is used | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // GPS (NL-552ETTL): | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 2D-Compass (HDMM01): | ||
| + | |||
| + | // MMC2120MG magnetometer: | ||
| + | #define I2C_MULTIIO_HDMM01_ADR 0x60 // Default | ||
| + | |||
| + | // MMC2120MG magnetometer calibration data: | ||
| + | #define MAX_X_2DM 2194.0 // Max. X-axis value | ||
| + | #define MIN_X_2DM 2026.0 // Min. X-axis value | ||
| + | #define BIAS_X_2DM (MAX_X_2DM + MIN_X_2DM) / 2.0 | ||
| + | #define SF_X_2DM 2.0 / (MAX_X_2DM - MIN_X_2DM) | ||
| + | #define MAX_Y_2DM -1892.0 // Max. Y-axis value | ||
| + | #define MIN_Y_2DM -2068.0 // Min. Y-axis value | ||
| + | #define BIAS_Y_2DM (MAX_Y_2DM + MIN_Y_2DM) / 2.0 | ||
| + | #define SF_Y_2DM 2.0 / (MAX_Y_2DM - MIN_Y_2DM) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 9D-IMU (MinIMU-9 v2): | ||
| + | |||
| + | // L3GD20 gyroscope: | ||
| + | #define I2C_MULTIIO_L3GD20_ADR 0xd6 // Default (SDO -> VCC) | ||
| + | //#define I2C_MULTIIO_L3GD20_ADR 0xd4 // SDO -> GND | ||
| + | |||
| + | // L3GD20 gyroscope calibration data: | ||
| + | #define MAX_X 20000 // Max. X-axis value | ||
| + | #define MIN_X -20000 // Min. X-axis value | ||
| + | #define OFFSET_X 0 // Offset X-axis | ||
| + | #define MAX_Y 20000 // Max. Y-axis value | ||
| + | #define MIN_Y -20000 // Min. Y-axis value | ||
| + | #define OFFSET_Y 0 // Offset Y-axis | ||
| + | #define MAX_Z 20000 // Max. Z-axis value | ||
| + | #define MIN_Z -20000 // Min. Z-axis value | ||
| + | #define OFFSET_Z 0 // Offset Z-axis | ||
| + | |||
| + | // L3GD20 temperature sensor definitions: | ||
| + | #define GET_TEMP // Use temperature sensor | ||
| + | #define OFFSET_TEMP 0 // Temperature offset [°C] | ||
| + | |||
| + | // LSM303DLHC accelerometer: | ||
| + | #define I2C_MULTIIO_LSM303DLHC_A_ADR 0x32 // Default | ||
| + | |||
| + | // LSM303DLHC accelerometer calibration data: | ||
| + | #define MAX_X_A 1019 // Max. X-axis value | ||
| + | #define MIN_X_A -1057 // Min. X-axis value | ||
| + | #define MAX_Y_A 1029 // Max. Y-axis value | ||
| + | #define MIN_Y_A -1064 // Min. Y-axis value | ||
| + | #define MAX_Z_A 965 // Max. Z-axis value | ||
| + | #define MIN_Z_A -1057 // Min. Z-axis value | ||
| + | #define OFFSET_PITCH_A 0.0 // Offset Pitch [°] | ||
| + | #define OFFSET_ROLL_A 0.0 // Offset Roll [°] | ||
| + | |||
| + | // LSM303DLHC magnetometer: | ||
| + | #define I2C_MULTIIO_LSM303DLHC_M_ADR 0x3c // Default | ||
| + | |||
| + | // LSM303DLHC magnetometer calibration data: | ||
| + | #define MAX_X_M 420.0 // Max. X-axis value | ||
| + | #define MIN_X_M -94.0 // Min. X-axis value | ||
| + | #define OFFSET_X_M 0 // Hard iron X-axis offset | ||
| + | #define MAX_Y_M 341.0 // Max. Y-axis value | ||
| + | #define MIN_Y_M -125.0 // Min. Y-axis value | ||
| + | #define OFFSET_Y_M 0 // Hard iron Y-axis offset | ||
| + | #define MAX_Z_M 467.0 // Max. Z-axis value | ||
| + | #define MIN_Z_M -30.0 // Min. Z-axis value | ||
| + | #define OFFSET_Z_M 0 // Hard iron Z-axis offset | ||
| + | #define DECLINATION_M 0.0 // E at local position [°] | ||
| + | |||
| + | // LSM303DLHC temperature sensor definitions: | ||
| + | #define GET_TEMP_M // Enable temperature sensor | ||
| + | #define OFFSET_TEMP_M 0.0 // Temperature offset [°C] | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #endif | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * | ||
| + | * ---> changes are documented in the file "RP6M256_OrientationLib.c" | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
=====Library Header===== | =====Library Header===== | ||
| + | Datei: RP6M256_OrientationLib.h: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 Orientation Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * Header file for new Orientation library. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | #ifndef RP6M256_ORIENTATIONLIB_H | ||
| + | #define RP6M256_ORIENTATIONLIB_H | ||
| + | |||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // The following sensors are used: | ||
| + | // - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS | ||
| + | // chipset | ||
| + | // - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG | ||
| + | // 2-axis magnetometer | ||
| + | // - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and | ||
| + | // the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" | ||
| + | #include "RP6I2CmasterTWI.h" | ||
| + | #include "RP6M256_Orientation.h" | ||
| + | #include "RP6M256uart1.h" // The RP6 M256 UART1 Library | ||
| + | #include "RP6M256_MultiIO.h" | ||
| + | #include <math.h> | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // GPS (NL-552ETTL): | ||
| + | |||
| + | #ifdef GPS | ||
| + | extern char parseBuffer[256]; | ||
| + | |||
| + | void GPS_init(void); | ||
| + | void clearParseBuffer(void); | ||
| + | uint8_t parseNMEAsentence(char *sentence, uint8_t data); | ||
| + | uint8_t checkNMEAsentence(void); | ||
| + | void parseNMEAitem(char *result, uint8_t index); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 2D-Compass (HDMM01): | ||
| + | |||
| + | // MMC2120MG magnetometer internal registers: | ||
| + | #define INTERNAL_REGISTER 0x00 | ||
| + | #define MSB_X_AXIS 0x01 | ||
| + | #define LSB_X_AXIS 0x02 | ||
| + | #define MSB_Y_AXIS 0x03 | ||
| + | #define LSB_Y_AXIS 0x04 | ||
| + | |||
| + | // MMC2120MG magnetometer internal register settings: | ||
| + | #define INTERNAL_REGISTER_TM 0b00000001 | ||
| + | #define INTERNAL_REGISTER_SET 0b00000010 | ||
| + | #define INTERNAL_REGISTER_RESET 0b00000100 | ||
| + | |||
| + | #ifdef COMPASS_2D | ||
| + | extern uint8_t intreg2dm; | ||
| + | extern int16_t x_axis2dm, y_axis2dm; | ||
| + | extern int16_t heading2dm; | ||
| + | extern double x2dm, y2dm; | ||
| + | |||
| + | void resetCoilHDMM01(void); | ||
| + | void COMPASS_2D_init(void); | ||
| + | uint8_t readHDMM01(void); | ||
| + | void normalizeHDMM01(void); | ||
| + | int16_t headingHDMM01(void); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 9D-IMU (MinIMU-9 v2): | ||
| + | |||
| + | // L3GD20 gyroscope internal registers: | ||
| + | #define WHO_AM_I 0x0f | ||
| + | #define CTRL_REG1 0x20 | ||
| + | #define CTRL_REG2 0x21 | ||
| + | #define CTRL_REG3 0x22 | ||
| + | #define CTRL_REG4 0x23 | ||
| + | #define CTRL_REG5 0x24 | ||
| + | #define REFERENCE 0x25 | ||
| + | #define OUT_TEMP 0x26 | ||
| + | #define STATUS_REG 0x27 | ||
| + | #define OUT_X_L 0x28 | ||
| + | #define OUT_X_H 0x29 | ||
| + | #define OUT_Y_L 0x2a | ||
| + | #define OUT_Y_H 0x2b | ||
| + | #define OUT_Z_L 0x2c | ||
| + | #define OUT_Z_H 0x2d | ||
| + | #define FIFO_CTRL_REG 0x2e | ||
| + | #define FIFO_SRC_REG 0x2f | ||
| + | #define INT1_CFG 0x30 | ||
| + | #define INT1_SRC 0x31 | ||
| + | #define INT1_TSH_XH 0x32 | ||
| + | #define INT1_TSH_XL 0x33 | ||
| + | #define INT1_TSH_YH 0x34 | ||
| + | #define INT1_TSH_YL 0x35 | ||
| + | #define INT1_TSH_ZH 0x36 | ||
| + | #define INT1_TSH_ZL 0x37 | ||
| + | #define INT1_DURATION 0x38 | ||
| + | |||
| + | // L3GD20 gyroscope register settings: | ||
| + | #define CTRL_REG1_DEFAULT 0b00000000 // Gyro Power-down | ||
| + | #define CTRL_REG1_SLEEP 0b00001000 // Gyro Sleep | ||
| + | #define CTRL_REG1_EN 0b00001111 // Gyro on, all axes enable | ||
| + | |||
| + | // LSM303DLHC accelerometer internal registers: | ||
| + | #define CTRL_REG1_A 0x20 | ||
| + | #define CTRL_REG2_A 0x21 | ||
| + | #define CTRL_REG3_A 0x22 | ||
| + | #define CTRL_REG4_A 0x23 | ||
| + | #define CTRL_REG5_A 0x24 | ||
| + | #define CTRL_REG6_A 0x25 | ||
| + | #define REFERENCE_A 0x26 | ||
| + | #define STATUS_REG_A 0x27 | ||
| + | #define OUT_X_L_A 0x28 | ||
| + | #define OUT_X_H_A 0x29 | ||
| + | #define OUT_Y_L_A 0x2a | ||
| + | #define OUT_Y_H_A 0x2b | ||
| + | #define OUT_Z_L_A 0x2c | ||
| + | #define OUT_Z_H_A 0x2d | ||
| + | #define FIFO_CTRL_REG_A 0x2e | ||
| + | #define FIFO_SRC_REG_A 0x2f | ||
| + | #define INT1_CFG_A 0x30 | ||
| + | #define INT1_SOURCE_A 0x31 | ||
| + | #define INT1_THS_A 0x32 | ||
| + | #define INT1_DURATION_A 0x33 | ||
| + | #define INT2_CFG_A 0x34 | ||
| + | #define INT2_SOURCE_A 0x35 | ||
| + | #define INT2_THS_A 0x36 | ||
| + | #define INT2_DURATION_A 0x37 | ||
| + | #define CLICK_CFG_A 0x38 | ||
| + | #define CLICK_SRC_A 0x39 | ||
| + | #define CLICK_THS_A 0x3a | ||
| + | #define TIME_LIMIT_A 0x3b | ||
| + | #define TIME_LATENCY_A 0x3c | ||
| + | #define TIME_WINDOW_A 0x3d | ||
| + | |||
| + | // LSM303DLHC accelerometer register settings: | ||
| + | #define CTRL_REG1_A_10HZ 0b00100111 // 10Hz & all axes enable | ||
| + | #define CTRL_REG1_A_50HZ 0b01000111 // 50Hz & all axes enable | ||
| + | #define CTRL_REG4_A_DEFAULT 0b00000000 // Default (Normal mode) | ||
| + | #define CTRL_REG4_A_HR 0b00001000 // HR output mode | ||
| + | |||
| + | // LSM303DLHC magnetometer internal registers: | ||
| + | #define CRA_REG_M 0x00 | ||
| + | #define CRB_REG_M 0x01 | ||
| + | #define MR_REG_M 0x02 | ||
| + | #define OUT_X_H_M 0x03 | ||
| + | #define OUT_X_L_M 0x04 | ||
| + | #define OUT_Z_H_M 0x05 | ||
| + | #define OUT_Z_L_M 0x06 | ||
| + | #define OUT_Y_H_M 0x07 | ||
| + | #define OUT_Y_L_M 0x08 | ||
| + | #define SR_REG_MG 0x09 | ||
| + | #define IRA_REG_M 0x0a | ||
| + | #define IRB_REG_M 0x0b | ||
| + | #define IRC_REG_M 0x0c | ||
| + | #define TEMP_OUT_H_M 0x31 | ||
| + | #define TEMP_OUT_L_M 0x32 | ||
| + | |||
| + | // LSM303DLHC magnetometer register settings: | ||
| + | #define CRA_REG_M_15HZ 0b00010000 // Default (Data rate 15Hz) | ||
| + | #define CRA_REG_M_15HZ_T_EN 0b10010000 // 15Hz & temperature enable | ||
| + | #define CRB_REG_M_13GAUSS 0b00100000 // Gain +-1.3Gauss | ||
| + | #define MR_REG_M_CCM 0b00000000 // Continuous-conversion mode | ||
| + | #define MR_REG_M_SCM 0b00000001 // Single-conversion mode | ||
| + | #define MR_REG_M_SLEEP 0b00000011 // Default (Sleep-mode) | ||
| + | |||
| + | #ifdef IMU_9D | ||
| + | extern int16_t x_axisg, y_axisg, z_axisg; | ||
| + | extern double xg, yg, zg; | ||
| + | #ifdef GET_TEMP | ||
| + | extern int8_t temperatureg; | ||
| + | #endif | ||
| + | |||
| + | extern int16_t x_axisa, y_axisa, z_axisa; | ||
| + | extern double pitch, roll; | ||
| + | extern double xa, ya, za; | ||
| + | extern double the, phi; | ||
| + | |||
| + | extern int16_t x_axism, y_axism, z_axism; | ||
| + | extern int16_t headingm; | ||
| + | extern double xm, ym, zm; | ||
| + | #ifdef GET_TEMP_M | ||
| + | extern int16_t temperaturem; | ||
| + | extern double temperature_imu; | ||
| + | #endif | ||
| + | |||
| + | extern int16_t headingtc; | ||
| + | |||
| + | void L3GD20_init(void); | ||
| + | void readL3GD20(void); | ||
| + | void normalizeL3GD20(void); | ||
| + | #ifdef GET_TEMP | ||
| + | int16_t map(int16_t, int16_t, int16_t, int16_t, int16_t); | ||
| + | #define calcTempL3GD20(__VALUE__) map(__VALUE__,127,-128,-40,85) | ||
| + | #endif | ||
| + | |||
| + | void LSM303DLHC_A_init(void); | ||
| + | void readLSM303DLHC_A(void); | ||
| + | void normalizeLSM303DLHC_A(void); | ||
| + | void positionLSM303DLHC_A(void); | ||
| + | |||
| + | void LSM303DLHC_M_init(void); | ||
| + | void readLSM303DLHC_M(void); | ||
| + | void normalizeLSM303DLHC_M(void); | ||
| + | int16_t headingLSM303DLHC_M(void); | ||
| + | |||
| + | int16_t headingLSM303DLHC_TC(void); | ||
| + | |||
| + | void IMU_9D_init(void); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Orientation library initialisation: | ||
| + | |||
| + | void orientation_init(void); | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #endif | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * | ||
| + | * ---> changes are documented in the file "RP6M256_OrientationLib.c" | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
| + | |||
=====Library Source===== | =====Library Source===== | ||
| + | |||
| + | Datei: RP6M256_OrientationLib.c: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 Orientation Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * | ||
| + | * This is our new Library that contains basic routines and functions for | ||
| + | * accessing certain sensors connected to the MultiIO Project Board. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // The following sensors are used: | ||
| + | // - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS | ||
| + | // chipset | ||
| + | // - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG | ||
| + | // 2-axis magnetometer | ||
| + | // - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and | ||
| + | // the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256_OrientationLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // GPS (NL-552ETTL): | ||
| + | |||
| + | #ifdef GPS | ||
| + | /** | ||
| + | * --------------------------------------------------------------------------- | ||
| + | * NMEA 0183 short description: | ||
| + | * 1. GPRMC: | ||
| + | * $GPRMC,213938.000,A,5108.3356,N,00951.0774,E,0.00,79.64,061012,,,A*53 | ||
| + | * Index: 1 2 3 4 5 6 7 8 9 12CS | ||
| + | * 1 = UTC of position [hhmmss.sss] | ||
| + | * 2 = Data status (A=ok, V=void) | ||
| + | * 3 = Latitude of fix [ddmm.mmmm] | ||
| + | * 4 = N or S | ||
| + | * 5 = Longitude of fix [dddmm.mmmm] | ||
| + | * 6 = E or W | ||
| + | * 7 = Speed over ground in knots | ||
| + | * 8 = Track made good, degrees true | ||
| + | * 9 = UT date [ddmmyy] | ||
| + | * 10 = Magnetic variation degrees (Easterly var. subtracts from true course) | ||
| + | * 11 = E or W | ||
| + | * 12 = Mode indicator (NMEA V2.3): | ||
| + | * A=Autonomous mode | ||
| + | * D=Differential mode | ||
| + | * E=Estimated (dead-reckoning) mode | ||
| + | * M=Manual input mode | ||
| + | * S=Simulated mode | ||
| + | * N=Data not valid | ||
| + | * CS = Checksum | ||
| + | * | ||
| + | * 2. GPVTG: | ||
| + | * $GPVTG,79.64,T,,M,0.00,N,0.0,K,A*31 | ||
| + | * Index: 1 2 4 5 6 7 8 9 CS | ||
| + | * 1 = Track made good | ||
| + | * 2 = Fixed text 'T' indicates that track made good is relative to true north | ||
| + | * 3 = Track degrees | ||
| + | * 4 = M=Magnetic | ||
| + | * 5 = Speed over ground in knots | ||
| + | * 6 = Fixed text 'N' indicates that speed over ground is in knots | ||
| + | * 7 = Speed over ground in kilometers/hour | ||
| + | * 8 = Fixed text 'K' indicates that speed over ground is in kilometers/hour | ||
| + | * 9 = Mode indicator (A,D,E,M,S,N) | ||
| + | * CS = Checksum | ||
| + | * | ||
| + | * 3. GPGGA: | ||
| + | * $GPGGA,213938.000,5108.3356,N,00951.0774,E,1,04,5.3,158.5,M,46.2,M,,0000*52 | ||
| + | * Index: 1 2 3 4 5 6 7 8 9 11 12 14 CS | ||
| + | * 1 = UTC of position | ||
| + | * 2 = Latitude of fix | ||
| + | * 3 = N or S | ||
| + | * 4 = Longitude of fix | ||
| + | * 5 = E or W | ||
| + | * 6 = GPS quality indicator: | ||
| + | * 0=invalid | ||
| + | * 1=GPS fix (SPS) | ||
| + | * 2=DGPS fix | ||
| + | * 3=PPS fix | ||
| + | * 4=Real Time Kinematic | ||
| + | * 5=Float RTK | ||
| + | * 6=estimated (dead reckoning) (NMEA V2.3) | ||
| + | * 7=Manual input mode | ||
| + | * 8=Simulation mode | ||
| + | * 7 = Number of satellites in use [not those in view] | ||
| + | * 8 = Horizontal dilution of position (HDOP) | ||
| + | * 9 = Antenna altitude above/below mean sea level (geoid) | ||
| + | * 10 = Meters (Antenna height unit) | ||
| + | * 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and | ||
| + | * mean sea level. -=geoid is below WGS-84 ellipsoid) | ||
| + | * 12 = Meters (Units of geoidal separation) | ||
| + | * 13 = Age in seconds since last update from diff. reference station | ||
| + | * 14 = Differential reference station ID# | ||
| + | * CS = Checksum | ||
| + | * | ||
| + | * 4. GPGSA: | ||
| + | * $GPGSA,A,3,02,04,10,07,,,,,,,,,7.4,5.3,5.1*33 | ||
| + | * Index: 1 2 3 4 5 6 15 16 17 CS | ||
| + | * 1 = Selection mode: | ||
| + | * M=Manual, forced to operate in 2D or 3D | ||
| + | * A=Automatic 2D/3D | ||
| + | * 2 = Mode: | ||
| + | * 1=no fix | ||
| + | * 2=2D | ||
| + | * 3=3D | ||
| + | * 3-14 = IDs of SVs used in position fix (null for unused fields) | ||
| + | * 15 = PDOP | ||
| + | * 16 = HDOP | ||
| + | * 17 = VDOP | ||
| + | * CS = Checksum | ||
| + | * | ||
| + | * 5. GPGSV: | ||
| + | * $GPGSV,3,1,12,02,36,301,37,04,53,235,21,05,03,294,,07,35,170,29*79 | ||
| + | * $GPGSV,3,2,12,08,04,187,28,10,56,290,36,13,88,106,20,16,12,076,*7D | ||
| + | * $GPGSV,3,3,12,20,23,118,11,23,50,067,,29,03,351,12,30,16,048,22*70 | ||
| + | * Index: 1 2 3 4 5 6 7 8 9 10 12 13 14 15 16 17 18 19 CS | ||
| + | * 1 = Total number of messages of this type in this cycle | ||
| + | * 2 = Message number | ||
| + | * 3 = Total number of SVs in view | ||
| + | * 4 = SV PRN number | ||
| + | * 5 = Elevation in degrees, 90 maximum | ||
| + | * 6 = Azimuth, degrees from true north, 000 to 359 | ||
| + | * 7 = SNR, 00-99 dB (null when not tracking) | ||
| + | * 8-11 = Information about second SV, same as field 4-7 | ||
| + | * 12-15= Information about third SV, same as field 4-7 | ||
| + | * 16-19= Information about fourth SV, same as field 4-7 | ||
| + | * CS = Checksum | ||
| + | * | ||
| + | * 6. GPGLL: | ||
| + | * $GPGLL,5108.3356,N,00951.0774,E,213938.000,A,A*5C | ||
| + | * Index: 1 2 3 4 5 6 7 CS | ||
| + | * 1 = Latitude of fix | ||
| + | * 2 = N or S | ||
| + | * 3 = Longitude of fix | ||
| + | * 4 = E or W | ||
| + | * 5 = UTC of position | ||
| + | * 6 = Data status (A=ok, V=void) | ||
| + | * 7 = Mode indicator (A,D,E,M,S,N) | ||
| + | * CS = Checksum | ||
| + | * | ||
| + | * COMMENTS: | ||
| + | * a) Each sentence (line) begins with '$'. | ||
| + | * b) Each sentence (line) ends with '*', followed by the checksum (two | ||
| + | * chars building an 8-bit HEX number) and terminated by <CR><LF>. | ||
| + | * c) The checksum (CS) is calculated by XOR of all chars between '$' and '*'. | ||
| + | * d) The max. sentence (line) length is 82 chars (including <CR><LF>). | ||
| + | * e) Further infos: | ||
| + | * -> http://www.nmea.org/ | ||
| + | * -> http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm | ||
| + | * -> http://de.wikipedia.org/wiki/NMEA_0183 | ||
| + | * -> http://aprs.gids.nl/nmea/ | ||
| + | * --------------------------------------------------------------------------- | ||
| + | * | ||
| + | * Call this once before using the GPS. | ||
| + | * | ||
| + | */ | ||
| + | void GPS_init(void) | ||
| + | { | ||
| + | cli(); // Disable global interrupts | ||
| + | // Initialisation of UART1: | ||
| + | UBRR1H = UBRR_BAUD_LOW >> 8; // Setup UART1: Baud is 38.4 kBaud | ||
| + | UBRR1L = (uint8_t) UBRR_BAUD_LOW; | ||
| + | UCSR1A = 0x00; | ||
| + | UCSR1C = (0<<UMSEL10) | (0<<UMSEL11) | (1<<UCSZ11) | (1<<UCSZ10); | ||
| + | UCSR1B = (1 << TXEN1) | (1 << RXEN1) | (1 << RXCIE1); | ||
| + | clearReceptionBuffer1(); | ||
| + | sei(); // Enable global interrupts | ||
| + | } | ||
| + | |||
| + | char parseBuffer[256]; | ||
| + | |||
| + | uint16_t parseBuffer_length = 0; | ||
| + | uint8_t parseBuffer_pos = 0; | ||
| + | |||
| + | /** | ||
| + | * Clears the parseBuffer | ||
| + | * | ||
| + | */ | ||
| + | void clearParseBuffer(void) | ||
| + | { | ||
| + | parseBuffer_pos = 0; | ||
| + | parseBuffer_length = 0; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Writes received byte data into buffer parseBuffer and returns if a full | ||
| + | * NMEA sentence has been received. Then the buffer is terminated with 0. | ||
| + | * Returns 0 if a sentence has been detected and buffer is ready. | ||
| + | * Returns 1 if no sentence has been received yet | ||
| + | * Returns 2 if buffer overflow occured | ||
| + | * | ||
| + | */ | ||
| + | uint8_t parseNMEAsentence(char *sentence, uint8_t data) | ||
| + | { | ||
| + | static uint8_t startflag = false; | ||
| + | static char last_data[10]; | ||
| + | uint16_t length = 0; | ||
| + | uint8_t i = 0; | ||
| + | |||
| + | if(data == 0) return 1; | ||
| + | while (sentence[i++]) length++; | ||
| + | if(length > 10) length = 10; | ||
| + | for(i = 0; i < 9; i++) last_data[i] = last_data[i + 1]; | ||
| + | last_data[9] = data; | ||
| + | parseBuffer[parseBuffer_pos] = data; | ||
| + | parseBuffer_pos++; | ||
| + | parseBuffer_length++; | ||
| + | if(parseBuffer_pos > 254) { | ||
| + | clearParseBuffer(); | ||
| + | startflag = false; | ||
| + | return 2; | ||
| + | } | ||
| + | if(!startflag && (strstr(last_data, sentence) != NULL)) { // Chosen sentence start | ||
| + | for(i = 0; i < length; i++) { | ||
| + | parseBuffer[i] = sentence[i]; | ||
| + | } | ||
| + | parseBuffer_pos = length; | ||
| + | parseBuffer_length = length; | ||
| + | startflag = true; | ||
| + | } | ||
| + | if(startflag) { | ||
| + | if(data == '\r') { // Chosen sentence end | ||
| + | parseBuffer[parseBuffer_pos] = 0; | ||
| + | last_data[9] = 0; | ||
| + | startflag = false; | ||
| + | return 0; | ||
| + | } | ||
| + | } | ||
| + | return 1; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Outputs true, if the NMEA sentence in parseBuffer is intact, | ||
| + | * else returns false. | ||
| + | * | ||
| + | */ | ||
| + | uint8_t checkNMEAsentence(void) | ||
| + | { | ||
| + | uint8_t i, cs; | ||
| + | char cs_str[3]; | ||
| + | |||
| + | if(parseBuffer_length < 18) return false; | ||
| + | if(parseBuffer_length > 82) return false; | ||
| + | if(parseBuffer[0] != '$') return false; | ||
| + | if(parseBuffer[parseBuffer_length - 4] != '*') return false; | ||
| + | cs = parseBuffer[1]; | ||
| + | for(i = 2; i < (parseBuffer_length - 4); i++) { | ||
| + | cs ^= parseBuffer[i]; // Calculate checksum | ||
| + | } | ||
| + | itoa(cs, &cs_str[0], HEX); | ||
| + | strupr(&cs_str[0]); // cs as uppercase chars | ||
| + | if((cs_str[0] != parseBuffer[parseBuffer_length - 3]) | ||
| + | || (cs_str[1] != parseBuffer[parseBuffer_length - 2])) return false; | ||
| + | return true; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Outputs a result string containing the data with index of the NMEA | ||
| + | * sentence in parseBuffer. | ||
| + | * | ||
| + | */ | ||
| + | void parseNMEAitem(char *result, uint8_t index) | ||
| + | { | ||
| + | uint8_t index_cnt, buffer_idx, result_idx; | ||
| + | |||
| + | index_cnt = index; | ||
| + | buffer_idx = 6; | ||
| + | while(index_cnt) { | ||
| + | if(parseBuffer[buffer_idx++] == ',') index_cnt--; | ||
| + | } | ||
| + | result_idx = 0; | ||
| + | while ((parseBuffer[buffer_idx] != ',') | ||
| + | && (parseBuffer[buffer_idx] != '*')) { | ||
| + | result[result_idx++] = parseBuffer[buffer_idx++]; | ||
| + | } | ||
| + | result[result_idx] = 0; | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 2D-Compass (HDMM01): | ||
| + | |||
| + | #ifdef COMPASS_2D | ||
| + | uint8_t intreg2dm; | ||
| + | int16_t x_axis2dm, y_axis2dm; | ||
| + | int16_t heading2dm; | ||
| + | double x2dm, y2dm; | ||
| + | |||
| + | /** | ||
| + | * This function sets and resets the internal coil of the | ||
| + | * magnetic sensor. | ||
| + | * | ||
| + | */ | ||
| + | void resetCoilHDMM01(void) | ||
| + | { | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER, INTERNAL_REGISTER_SET); | ||
| + | mSleep(1); | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER, INTERNAL_REGISTER_RESET); | ||
| + | mSleep(5); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the 2D-Compass. | ||
| + | * | ||
| + | */ | ||
| + | void COMPASS_2D_init(void) | ||
| + | { | ||
| + | // Reset coil: | ||
| + | resetCoilHDMM01(); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function reads the X-axis and Y-axis values from the | ||
| + | * HDMM01 and stores them in the global variables x_axis2dm | ||
| + | * and y_axis2dm. | ||
| + | * It returns the Internal register of the HDMM01. | ||
| + | * | ||
| + | */ | ||
| + | uint8_t readHDMM01(void) | ||
| + | { | ||
| + | uint8_t readBuf[5]; | ||
| + | |||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER, INTERNAL_REGISTER_TM); | ||
| + | mSleep(5); | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_HDMM01_ADR, readBuf, 5); // Read Int. reg. & X-/Y-axis | ||
| + | // xb = y: | ||
| + | x_axis2dm = ((readBuf[MSB_Y_AXIS] & 0x0f) << 8) + readBuf[LSB_Y_AXIS]; | ||
| + | // yb = -x: | ||
| + | y_axis2dm = ((readBuf[MSB_X_AXIS] & 0x0f) << 8) + readBuf[LSB_X_AXIS]; | ||
| + | y_axis2dm *= -1; | ||
| + | return readBuf[INTERNAL_REGISTER]; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function scales the x-axis2dm and y-axis2dm | ||
| + | * values read from the HDMM01 with the function | ||
| + | * readHDMM01() and stores them in the global double | ||
| + | * variables x2dm and y2dm. | ||
| + | * | ||
| + | */ | ||
| + | void normalizeHDMM01(void) | ||
| + | { | ||
| + | x2dm = (x_axis2dm - BIAS_X_2DM) * SF_X_2DM; | ||
| + | y2dm = (y_axis2dm - BIAS_Y_2DM) * SF_Y_2DM; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function calculates the heading by using the | ||
| + | * normalized sensor values x2dm and y2dm (see function | ||
| + | * normalizeHDMM01()!). The heading is a value from 0° | ||
| + | * to 359°. If the robot's front (and the sensor's | ||
| + | * SDA/SCL pin side) points to ... | ||
| + | * north -> heading is 0°, | ||
| + | * east -> heading is 90°, | ||
| + | * south -> heading is 180°, | ||
| + | * west -> heading is 270°. | ||
| + | * | ||
| + | */ | ||
| + | int16_t headingHDMM01(void) | ||
| + | { | ||
| + | double hdg = atan2(y2dm, x2dm) * 180 / M_PI; | ||
| + | if(hdg < 0.0) hdg += 360.0; | ||
| + | return ((int16_t) hdg); | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 9D-IMU (MinIMU-9 v2): | ||
| + | |||
| + | #ifdef IMU_9D | ||
| + | // -------------------------------------------- | ||
| + | // L3GD20 gyroscope functions: | ||
| + | int16_t x_axisg, y_axisg, z_axisg; | ||
| + | double xg, yg, zg; | ||
| + | #ifdef GET_TEMP | ||
| + | int8_t temperatureg; | ||
| + | #endif | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the 3D-Gyro. | ||
| + | * | ||
| + | */ | ||
| + | void L3GD20_init(void) | ||
| + | { | ||
| + | // Set normal power mode, all axes enabled: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG1, CTRL_REG1_EN); | ||
| + | mSleep(10); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function reads the X-axis, Y-axis, Z-axis and | ||
| + | * temperature values from the L3GD20 gyroscope and stores | ||
| + | * them in the global variables x_axisg, y_axisg, z_axisg | ||
| + | * and temperatureg. | ||
| + | * | ||
| + | */ | ||
| + | void readL3GD20(void) | ||
| + | { | ||
| + | uint8_t readBuf[6]; | ||
| + | |||
| + | I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80)); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read X-/Y-/Z-axis | ||
| + | // xb = y: | ||
| + | x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L]; | ||
| + | // yb = -x: | ||
| + | y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L]; | ||
| + | y_axisg *= -1; | ||
| + | // zb = z: | ||
| + | z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L]; | ||
| + | mSleep(10); | ||
| + | #ifdef GET_TEMP | ||
| + | // Read temperature raw value: | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, OUT_TEMP); | ||
| + | temperatureg = I2CTWI_readByte(I2C_MULTIIO_L3GD20_ADR); | ||
| + | mSleep(10); | ||
| + | #endif | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function adds zero offsets and limits the x-axisg, | ||
| + | * y-axisg and z-axisg values read from the L3GD20 | ||
| + | * gyroscope with the function readL3GD20() and stores them | ||
| + | * in the global double variables xg, yg and zg. | ||
| + | * | ||
| + | */ | ||
| + | void normalizeL3GD20(void) | ||
| + | { | ||
| + | // Add zero offsets: | ||
| + | x_axisg += OFFSET_X; | ||
| + | y_axisg += OFFSET_Y; | ||
| + | z_axisg += OFFSET_Z; | ||
| + | // Limit raw values: | ||
| + | if (x_axisg < MIN_X) x_axisg = MIN_X; | ||
| + | if (x_axisg > MAX_X) x_axisg = MAX_X; | ||
| + | if (y_axisg < MIN_Y) y_axisg = MIN_Y; | ||
| + | if (y_axisg > MAX_Y) y_axisg = MAX_Y; | ||
| + | if (z_axisg < MIN_Z) z_axisg = MIN_Z; | ||
| + | if (z_axisg > MAX_Z) z_axisg = MAX_Z; | ||
| + | xg = (double) x_axisg; | ||
| + | yg = (double) y_axisg; | ||
| + | zg = (double) z_axisg; | ||
| + | } | ||
| + | |||
| + | #ifdef GET_TEMP | ||
| + | /** | ||
| + | * Returns the integer mapping function result. | ||
| + | * | ||
| + | * Input: value -> Source value | ||
| + | * fromSource -> Low source limit | ||
| + | * toSource -> High source limit | ||
| + | * fromTarget -> Low target limit | ||
| + | * toTarget -> High target limit | ||
| + | * | ||
| + | * There is also a macro calcTempL3GD20(), which | ||
| + | * calculates the temperature [°C] from the L3GD20 | ||
| + | * temperature output value (temperatureg) using this | ||
| + | * mapping function. | ||
| + | * | ||
| + | * Example: | ||
| + | * tmp = calcTempL3GD20(temperatureg); | ||
| + | * // This calculates the temperature [°C]! | ||
| + | * | ||
| + | */ | ||
| + | int16_t map(int16_t value, int16_t fromSource, int16_t toSource, int16_t fromTarget, int16_t toTarget) | ||
| + | { | ||
| + | double temp; | ||
| + | temp = ((double) (value - fromSource) / (toSource - fromSource) \ | ||
| + | * (toTarget - fromTarget) + fromTarget); | ||
| + | return (int16_t) temp; | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | // -------------------------------------------- | ||
| + | // LSM303DLHC accelerometer functions: | ||
| + | int16_t x_axisa, y_axisa, z_axisa; | ||
| + | double pitch, roll; | ||
| + | double xa, ya, za; | ||
| + | double the, phi; | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the 3D-Accelerometer. | ||
| + | * | ||
| + | */ | ||
| + | void LSM303DLHC_A_init(void) | ||
| + | { | ||
| + | // Set normal power mode, 10 Hz data rate, all axes enabled: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_A_ADR, CTRL_REG1_A, CTRL_REG1_A_10HZ); | ||
| + | mSleep(10); | ||
| + | // Set HD mode: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_A_ADR, CTRL_REG4_A, CTRL_REG4_A_HR); | ||
| + | mSleep(10); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function reads the X-axis, Y-axis and Z-axis values | ||
| + | * from the LSM303DLHC accelerometer and stores them in the | ||
| + | * global variables x_axisa, y_axisa and z_axisa. | ||
| + | * | ||
| + | */ | ||
| + | void readLSM303DLHC_A(void) | ||
| + | { | ||
| + | uint8_t readBuf[6]; | ||
| + | |||
| + | I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_A_ADR, (OUT_X_L_A | 0x80)); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_LSM303DLHC_A_ADR, readBuf, 6); // Read X-/Y-/Z-axis | ||
| + | // xb = -y: | ||
| + | x_axisa = ((readBuf[OUT_Y_H_A - OUT_X_L_A] << 8) + readBuf[OUT_Y_L_A - OUT_X_L_A]) / 16; | ||
| + | x_axisa *= -1; | ||
| + | // yb = x: | ||
| + | y_axisa = ((readBuf[OUT_X_H_A - OUT_X_L_A] << 8) + readBuf[OUT_X_L_A - OUT_X_L_A]) / 16; | ||
| + | // zb = -z: | ||
| + | z_axisa = ((readBuf[OUT_Z_H_A - OUT_X_L_A] << 8) + readBuf[OUT_Z_L_A - OUT_X_L_A]) / 16; | ||
| + | z_axisa *= -1; | ||
| + | mSleep(10); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function limits and normalizes the x-axisa, | ||
| + | * y-axisa and z-axisa values read from the LSM303DLHC | ||
| + | * accelerometer with the function readLSM303DLHC_A() and | ||
| + | * stores them in the global double variables xa, ya and | ||
| + | * za. | ||
| + | * It also calculates the position in space in form of the | ||
| + | * tilt angles (the = pitch, phi = roll) and stores them | ||
| + | * in the global double variables the and phi. | ||
| + | * | ||
| + | */ | ||
| + | void normalizeLSM303DLHC_A(void) | ||
| + | { | ||
| + | // Limit raw values: | ||
| + | if (x_axisa < MIN_X_A) x_axisa = MIN_X_A; | ||
| + | if (x_axisa > MAX_X_A) x_axisa = MAX_X_A; | ||
| + | if (y_axisa < MIN_Y_A) y_axisa = MIN_Y_A; | ||
| + | if (y_axisa > MAX_Y_A) y_axisa = MAX_Y_A; | ||
| + | if (z_axisa < MIN_Z_A) z_axisa = MIN_Z_A; | ||
| + | if (z_axisa > MAX_Z_A) z_axisa = MAX_Z_A; | ||
| + | xa = (double) x_axisa / 1000.0; | ||
| + | ya = (double) y_axisa / 1000.0; | ||
| + | za = (double) z_axisa / 1000.0; | ||
| + | // Calculate Pitch and Roll: | ||
| + | the = asin(-xa); // Calculate the and phi | ||
| + | phi = asin(ya / cos(the)); // from the raw values | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function calculates the position in space by | ||
| + | * using the the and phi values (see function | ||
| + | * normalizeLSM303DLHC_A()!). It stores the position | ||
| + | * in the global double variables pitch and roll. | ||
| + | * | ||
| + | */ | ||
| + | void positionLSM303DLHC_A(void) | ||
| + | { | ||
| + | pitch = the * 180 / M_PI + OFFSET_PITCH_A; | ||
| + | roll = phi * 180 / M_PI + OFFSET_ROLL_A; | ||
| + | } | ||
| + | |||
| + | // -------------------------------------------- | ||
| + | // LSM303DLHC magnetometer functions: | ||
| + | int16_t x_axism, y_axism, z_axism; | ||
| + | int16_t headingm; | ||
| + | double xm, ym, zm; | ||
| + | #ifdef GET_TEMP_M | ||
| + | int16_t temperaturem; | ||
| + | double temperature_imu; | ||
| + | #endif | ||
| + | |||
| + | int16_t headingtc; | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the 3D-Compass. | ||
| + | * | ||
| + | */ | ||
| + | void LSM303DLHC_M_init(void) | ||
| + | { | ||
| + | #ifdef GET_TEMP_M | ||
| + | // Set Data output rate to 15Hz, enable temperature sensor: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ_T_EN); | ||
| + | #else | ||
| + | // Set Data output rate to 15Hz: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ); | ||
| + | #endif | ||
| + | mSleep(10); | ||
| + | // Set Gain to +-1.3Gauss: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, CRB_REG_M, CRB_REG_M_13GAUSS); | ||
| + | mSleep(10); | ||
| + | // Set Continuous-conversion mode: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, MR_REG_M, MR_REG_M_CCM); | ||
| + | mSleep(10); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function reads the X-axis, Y-axis, Z-axis and | ||
| + | * temperature values from the LSM303DLHC magnetometer | ||
| + | * and stores them in the global variables x_axism, | ||
| + | * y_axism, z_axism and temperaturem. | ||
| + | * | ||
| + | */ | ||
| + | void readLSM303DLHC_M(void) | ||
| + | { | ||
| + | uint8_t readBuf[6]; | ||
| + | |||
| + | I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_M_ADR, OUT_X_H_M); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_LSM303DLHC_M_ADR, readBuf, 6); // Read X-/Y-/Z-axis | ||
| + | // xb = y: | ||
| + | x_axism = (readBuf[OUT_Y_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Y_L_M - OUT_X_H_M]; | ||
| + | // yb = x: | ||
| + | y_axism = (readBuf[OUT_X_H_M - OUT_X_H_M] << 8) + readBuf[OUT_X_L_M - OUT_X_H_M]; | ||
| + | // zb = -z: | ||
| + | z_axism = (readBuf[OUT_Z_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Z_L_M - OUT_X_H_M]; | ||
| + | z_axism *= -1; | ||
| + | mSleep(10); | ||
| + | #ifdef GET_TEMP_M | ||
| + | // Read temperature raw value: | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_M_ADR, TEMP_OUT_H_M); | ||
| + | readBuf[0] = I2CTWI_readByte(I2C_MULTIIO_LSM303DLHC_M_ADR); | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_M_ADR, TEMP_OUT_L_M); | ||
| + | readBuf[1] = I2CTWI_readByte(I2C_MULTIIO_LSM303DLHC_M_ADR); | ||
| + | temperaturem = ((readBuf[0] << 8) + readBuf[1]) / 16; | ||
| + | mSleep(10); | ||
| + | #endif | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function compensates hard iron effects and | ||
| + | * scales the x-axism, y-axism and z-axism values read | ||
| + | * from the LSM303DLHC magnetometer with the function | ||
| + | * readLSM303DLHC_M() and stores them in the global | ||
| + | * double variables xm, ym and zm. | ||
| + | * | ||
| + | */ | ||
| + | void normalizeLSM303DLHC_M(void) | ||
| + | { | ||
| + | // Add hard iron offsets: | ||
| + | x_axism += OFFSET_X_M; | ||
| + | y_axism += OFFSET_Y_M; | ||
| + | z_axism += OFFSET_Z_M; | ||
| + | // Scale raw values: | ||
| + | xm = (x_axism - MIN_X_M) / (MAX_X_M - MIN_X_M) * 2 - 1; | ||
| + | ym = (y_axism - MIN_Y_M) / (MAX_Y_M - MIN_Y_M) * 2 - 1; | ||
| + | zm = (z_axism - MIN_Z_M) / (MAX_Z_M - MIN_Z_M) * 2 - 1; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function calculates the heading by using the | ||
| + | * normalized sensor values xm and ym (see function | ||
| + | * normalizeLSM303DLHC_M()!). The heading is a value | ||
| + | * from 0° to 359°. If the robot's front (and the | ||
| + | * sensor's upper edge with view on the printed | ||
| + | * side) points to ... | ||
| + | * north -> heading is 0°, | ||
| + | * east -> heading is 90°, | ||
| + | * south -> heading is 180°, | ||
| + | * west -> heading is 270°. | ||
| + | * | ||
| + | */ | ||
| + | int16_t headingLSM303DLHC_M(void) | ||
| + | { | ||
| + | double hdg = atan2(ym, xm) * 180 / M_PI + DECLINATION_M; | ||
| + | if(hdg < 0.0) hdg += 360.0; | ||
| + | return ((int16_t) hdg); | ||
| + | } | ||
| + | |||
| + | // -------------------------------------------- | ||
| + | /** | ||
| + | * This function calculates the TILT COMPENSATED | ||
| + | * heading by using accelerometer (the, phi) AND | ||
| + | * magnetometer (xm, ym, zm) data. The heading is a | ||
| + | * value from 0° to 359°. If the robot's front (and | ||
| + | * the sensor's upper edge with view on the printed | ||
| + | * side) points to ... | ||
| + | * north -> heading is 0°, | ||
| + | * east -> heading is 90°, | ||
| + | * south -> heading is 180°, | ||
| + | * west -> heading is 270°. | ||
| + | * | ||
| + | */ | ||
| + | int16_t headingLSM303DLHC_TC(void) | ||
| + | { | ||
| + | double hdgtc, xh, yh; | ||
| + | |||
| + | xh = xm * cos(the) + zm * sin(the); | ||
| + | yh = xm * sin(phi) * sin(the) + ym * cos(phi) - zm * sin(phi) * cos(the); | ||
| + | // zh = -xm * cos(phi) * sin(the) + ym * sin(phi) + zm * cos(phi) * cos(the); | ||
| + | hdgtc = atan2(yh, xh) * 180 / M_PI + DECLINATION_M; | ||
| + | if(hdgtc < 0.0) hdgtc += 360.0; | ||
| + | return ((int16_t) hdgtc); | ||
| + | } | ||
| + | |||
| + | // -------------------------------------------- | ||
| + | /** | ||
| + | * Call this once before using the 9D-IMU. | ||
| + | * | ||
| + | */ | ||
| + | void IMU_9D_init(void) | ||
| + | { | ||
| + | L3GD20_init(); // Gyroscope init | ||
| + | LSM303DLHC_A_init(); // Accelerometer init | ||
| + | LSM303DLHC_M_init(); // Magnetometer init | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Orientation library initialisation: | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the GPS module, | ||
| + | * the 2D-Compass or the 9D-IMU. | ||
| + | * | ||
| + | */ | ||
| + | void orientation_init(void) | ||
| + | { | ||
| + | #ifdef GPS | ||
| + | GPS_init(); | ||
| + | #endif | ||
| + | #ifdef COMPASS_2D | ||
| + | COMPASS_2D_init(); | ||
| + | #endif | ||
| + | #ifdef IMU_9D | ||
| + | IMU_9D_init(); | ||
| + | #endif | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * - v. 1.1 (smaller bugs fixed) 18.07.2013 by Dirk | ||
| + | * - v. 1.0 (initial release) 14.04.2013 by Dirk | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
| + | |||
======Erklärung====== | ======Erklärung====== | ||
| + | |||
=====Demo===== | =====Demo===== | ||
| + | |||
| + | makefile: | ||
| + | <pre>... | ||
| + | TARGET = RP6M256_MultiIO_05 | ||
| + | ... | ||
| + | SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256uart1.c | ||
| + | SRC += RP6M256_OrientationLib.c | ||
| + | ...</pre> | ||
| + | |||
| + | Datei: RP6M256_MultiIO_05.c | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 MultiIO | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * In this example we show a fifth test for the MultiIO Project Board. | ||
| + | * It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU). | ||
| + | * | ||
| + | * The following sensors are used: | ||
| + | * - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS | ||
| + | * chipset | ||
| + | * - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG | ||
| + | * 2-axis magnetometer | ||
| + | * - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and | ||
| + | * the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer | ||
| + | * | ||
| + | * ############################################################################ | ||
| + | * The Robot does NOT move in this example! You can simply put it on a table | ||
| + | * next to your PC and you should connect it to the PC via the USB Interface! | ||
| + | * You should also connect to it via WIFI. | ||
| + | * ############################################################################ | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" // The RP6 M256 Library. | ||
| + | // Always needs to be included! | ||
| + | #include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library | ||
| + | #include "RP6M256uart1.h" // The RP6 M256 UART1 Library | ||
| + | #include <math.h> | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Include our new "RP6M256 Orientation library": | ||
| + | // (This is the library for accessing certain sensors connected to the | ||
| + | // MultiIO Project Board!) | ||
| + | |||
| + | #include "RP6M256_OrientationLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Variables: | ||
| + | |||
| + | uint8_t ch; | ||
| + | char item[12]; | ||
| + | char dir[3]; | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /** | ||
| + | * Returns a 2 character string for the eighth | ||
| + | * parts of the direction calculated from the | ||
| + | * heading value. | ||
| + | * | ||
| + | * Input: heading -> Heading value [0..359] | ||
| + | * | ||
| + | */ | ||
| + | void calculateDir(char *dir, uint16_t heading) | ||
| + | { | ||
| + | dir[1] = ' '; | ||
| + | dir[2] = '\0'; | ||
| + | if ((heading <= 22) || (heading >=338)) dir[0] = 'N'; | ||
| + | if ((heading >= 23) && (heading <= 67)) {dir[0] = 'N'; dir[1] = 'E';} | ||
| + | if ((heading >= 68) && (heading <= 112)) dir[0] = 'E'; | ||
| + | if ((heading >= 113) && (heading <= 157)) {dir[0] = 'S'; dir[1] = 'E';} | ||
| + | if ((heading >= 158) && (heading <= 202)) dir[0] = 'S'; | ||
| + | if ((heading >= 203) && (heading <= 247)) {dir[0] = 'S'; dir[1] = 'W';} | ||
| + | if ((heading >= 248) && (heading <= 292)) dir[0] = 'W'; | ||
| + | if ((heading >= 293) && (heading <= 337)) {dir[0] = 'N'; dir[1] = 'W';} | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Write a floating point number to the WIFI. | ||
| + | * | ||
| + | * Example: | ||
| + | * | ||
| + | * // Write a floating point number to the WIFI (no exponent): | ||
| + | * writeDouble_WIFI(1234567.890, 11, 3); | ||
| + | * | ||
| + | * The value of prec (precision) defines the number of decimal places. | ||
| + | * For 32 bit floating point variables (float, double ...) 6 is | ||
| + | * the max. value for prec (7 relevant digits). | ||
| + | * The value of width defines the overall number of characters in the | ||
| + | * floating point number including the decimal point. The number of | ||
| + | * pre-decimal positions is: (width - prec - 1). | ||
| + | */ | ||
| + | void writeDouble_WIFI(double number, uint8_t width, uint8_t prec) | ||
| + | {char buffer[width + 1]; | ||
| + | dtostrf(number, width, prec, &buffer[0]); | ||
| + | writeString_WIFI(&buffer[0]); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Write a floating point number to the LCD. | ||
| + | * | ||
| + | * Example: | ||
| + | * | ||
| + | * // Write a floating point number to the LCD (no exponent): | ||
| + | * writeDoubleLCD(1234567.890, 11, 3); | ||
| + | * | ||
| + | * The value of prec (precision) defines the number of decimal places. | ||
| + | * For 32 bit floating point variables (float, double ...) 6 is | ||
| + | * the max. value for prec (7 relevant digits). | ||
| + | * The value of width defines the overall number of characters in the | ||
| + | * floating point number including the decimal point. The number of | ||
| + | * pre-decimal positions is: (width - prec - 1). | ||
| + | */ | ||
| + | void writeDoubleLCD(double number, uint8_t width, uint8_t prec) | ||
| + | {char buffer[width + 1]; | ||
| + | dtostrf(number, width, prec, &buffer[0]); | ||
| + | writeStringLCD(&buffer[0]); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Error handler | ||
| + | |||
| + | /** | ||
| + | * This function gets called automatically if there was an I2C Error like | ||
| + | * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30). | ||
| + | * | ||
| + | */ | ||
| + | void I2C_transmissionError(uint8_t errorState) | ||
| + | { | ||
| + | writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x"); | ||
| + | writeInteger_WIFI(errorState, HEX); | ||
| + | writeChar_WIFI('\n'); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Main function - The program starts here: | ||
| + | |||
| + | int main(void) | ||
| + | { | ||
| + | initRP6M256(); // Always call this first! The Processor will not work | ||
| + | // correctly otherwise. | ||
| + | |||
| + | initLCD(); // Initialize the LC-Display (LCD) | ||
| + | // Always call this before using the LCD! | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | mSleep(500); | ||
| + | setLEDs(0b0000); | ||
| + | |||
| + | writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5!\n"); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation | ||
| + | // with 100kHz SCL Frequency | ||
| + | |||
| + | |||
| + | // Register the event handler: | ||
| + | I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | |||
| + | // Write a text message to the LCD: | ||
| + | showScreenLCD("################", "################"); | ||
| + | mSleep(1500); | ||
| + | showScreenLCD("RP6v2-M256-WIFI ", "Example Program"); | ||
| + | mSleep(2500); | ||
| + | showScreenLCD("RP6M256 Multi IO", " Selftest 5"); | ||
| + | mSleep(2500); | ||
| + | clearLCD(); | ||
| + | |||
| + | setLEDs(0b0000); | ||
| + | |||
| + | // --------------------------------------- | ||
| + | |||
| + | startStopwatch1(); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | orientation_init(); // Orientation init! | ||
| + | |||
| + | while(true) | ||
| + | { | ||
| + | if(getStopwatch1() > 3000) // 3s | ||
| + | { | ||
| + | #ifdef GPS | ||
| + | // GPS test: | ||
| + | clearParseBuffer(); | ||
| + | clearReceptionBuffer1(); | ||
| + | writeString_P_WIFI("\nGPS SENSOR ->\n"); | ||
| + | while(parseNMEAsentence("$GPGGA", ch)) { | ||
| + | ch = readChar1(); | ||
| + | } | ||
| + | writeString_P_WIFI("GPGGA NMEA sentence:\n"); | ||
| + | writeString_WIFI(parseBuffer); | ||
| + | writeChar_WIFI('\n'); | ||
| + | if(checkNMEAsentence()) { | ||
| + | writeString_P_WIFI("Sentence OK!\n"); | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | parseNMEAitem(item, 3); | ||
| + | writeStringLCD(item); | ||
| + | writeString_P_WIFI("Position "); | ||
| + | writeString_WIFI(item); | ||
| + | parseNMEAitem(item, 2); | ||
| + | writeStringLCD(item); | ||
| + | writeString_WIFI(item); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeCharLCD('m'); | ||
| + | parseNMEAitem(item, 9); | ||
| + | writeStringLCD(item); | ||
| + | writeString_P_WIFI("Height [m]: "); | ||
| + | writeString_WIFI(item); | ||
| + | writeChar_WIFI('\n'); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | parseNMEAitem(item, 5); | ||
| + | writeStringLCD(item); | ||
| + | writeString_P_WIFI("Position "); | ||
| + | writeString_WIFI(item); | ||
| + | parseNMEAitem(item, 4); | ||
| + | writeStringLCD(item); | ||
| + | writeString_WIFI(item); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeCharLCD('s'); | ||
| + | parseNMEAitem(item, 7); | ||
| + | writeStringLCD(item); | ||
| + | writeString_P_WIFI("Satellites: "); | ||
| + | writeString_WIFI(item); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeCharLCD('q'); | ||
| + | parseNMEAitem(item, 6); | ||
| + | writeStringLCD(item); | ||
| + | } | ||
| + | else { | ||
| + | writeString_P_WIFI("Sentence NOT OK!\n"); | ||
| + | showScreenLCD("Sentence NOT OK!", "----------------"); | ||
| + | } | ||
| + | mSleep(3000); | ||
| + | #endif | ||
| + | #ifdef COMPASS_2D | ||
| + | // 2D-Compass test: | ||
| + | task_I2CTWI(); | ||
| + | intreg2dm = readHDMM01(); // Get sensor values | ||
| + | task_I2CTWI(); | ||
| + | writeString_P_WIFI("\n2D-Compass SENSOR ->\n"); | ||
| + | writeString_P_WIFI("Internal register: "); | ||
| + | writeIntegerLength_WIFI(intreg2dm, BIN, 8); | ||
| + | writeChar_WIFI('\n'); | ||
| + | if(!intreg2dm) { // Data valid? | ||
| + | writeString_P_WIFI("X-axis: "); | ||
| + | writeInteger_WIFI(x_axis2dm, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Y-axis: "); | ||
| + | writeInteger_WIFI(y_axis2dm, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | normalizeHDMM01(); // Normalize data | ||
| + | heading2dm = headingHDMM01(); // Calculate heading | ||
| + | writeString_P_WIFI("Heading [°]: "); | ||
| + | writeInteger_WIFI(heading2dm, DEC); | ||
| + | calculateDir(dir, heading2dm); | ||
| + | writeString_P_WIFI(" Direction: "); | ||
| + | writeString_WIFI(dir); | ||
| + | writeChar_WIFI('\n'); | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("Heading "); | ||
| + | writeIntegerLCD(heading2dm, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | writeStringLCD(dir); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("X "); | ||
| + | writeIntegerLCD(x_axis2dm, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 8); // line 2 pos 9 | ||
| + | writeStringLCD_P("Y "); | ||
| + | writeIntegerLCD(y_axis2dm, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | } | ||
| + | task_I2CTWI(); | ||
| + | mSleep(3000); | ||
| + | #endif | ||
| + | #ifdef IMU_9D | ||
| + | // 9D-IMU test: | ||
| + | // L3GD20 gyroscope: | ||
| + | task_I2CTWI(); | ||
| + | readL3GD20(); // Get sensor values | ||
| + | normalizeL3GD20(); | ||
| + | task_I2CTWI(); | ||
| + | writeString_P_WIFI("\n3D-Gyro SENSOR ->\n"); | ||
| + | writeString_P_WIFI("X-axis: "); | ||
| + | writeInteger_WIFI(x_axisg, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Y-axis: "); | ||
| + | writeInteger_WIFI(y_axisg, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Z-axis: "); | ||
| + | writeInteger_WIFI(z_axisg, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | #ifdef GET_TEMP | ||
| + | temperatureg = calcTempL3GD20(temperatureg) + 5; | ||
| + | temperatureg += OFFSET_TEMP; | ||
| + | writeString_P_WIFI("Temperature [°]: "); | ||
| + | writeInteger_WIFI(temperatureg, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | #endif | ||
| + | // LSM303DLHC accelerometer: | ||
| + | task_I2CTWI(); | ||
| + | readLSM303DLHC_A(); // Get sensor values | ||
| + | task_I2CTWI(); | ||
| + | writeString_P_WIFI("\n3D-Acceleration SENSOR ->\n"); | ||
| + | writeString_P_WIFI("X-axis: "); | ||
| + | writeInteger_WIFI(x_axisa, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Y-axis: "); | ||
| + | writeInteger_WIFI(y_axisa, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Z-axis: "); | ||
| + | writeInteger_WIFI(z_axisa, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | normalizeLSM303DLHC_A(); // Normalize data | ||
| + | positionLSM303DLHC_A(); // Calculate position | ||
| + | writeString_P_WIFI("POSITION: \n"); | ||
| + | writeString_P_WIFI(" Pitch [°]: "); | ||
| + | writeDouble_WIFI(pitch, 6, 1); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI(" Roll [°]: "); | ||
| + | writeDouble_WIFI(roll, 6, 1); | ||
| + | writeChar_WIFI('\n'); | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("P"); | ||
| + | writeDoubleLCD(pitch, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(0, 8); // line 1 pos 9 | ||
| + | writeStringLCD_P("R"); | ||
| + | writeDoubleLCD(roll, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("X"); | ||
| + | writeIntegerLCD(x_axisa, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 5); // line 2 pos 6 | ||
| + | writeStringLCD_P("Y"); | ||
| + | writeIntegerLCD(y_axisa, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 10); // line 2 pos 11 | ||
| + | writeStringLCD_P("Z"); | ||
| + | writeIntegerLCD(z_axisa, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | task_I2CTWI(); | ||
| + | mSleep(3000); | ||
| + | // LSM303DLHC magnetometer: | ||
| + | task_I2CTWI(); | ||
| + | readLSM303DLHC_M(); // Get sensor values | ||
| + | task_I2CTWI(); | ||
| + | writeString_P_WIFI("\n3D-Compass SENSOR ->\n"); | ||
| + | writeString_P_WIFI("X-axis: "); | ||
| + | writeInteger_WIFI(x_axism, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Y-axis: "); | ||
| + | writeInteger_WIFI(y_axism, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | writeString_P_WIFI("Z-axis: "); | ||
| + | writeInteger_WIFI(z_axism, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | normalizeLSM303DLHC_M(); // Normalize data | ||
| + | headingm = headingLSM303DLHC_M(); // Calculate heading | ||
| + | writeString_P_WIFI(" Heading [°]: "); | ||
| + | writeInteger_WIFI(headingm, DEC); | ||
| + | calculateDir(dir, headingm); | ||
| + | writeString_P_WIFI(" Direction: "); | ||
| + | writeString_WIFI(dir); | ||
| + | writeChar_WIFI('\n'); | ||
| + | headingtc = headingLSM303DLHC_TC(); // Calculate TILT COMPENSATED | ||
| + | writeString_P_WIFI("TILT COMPENSATED heading [°]: "); // heading | ||
| + | writeInteger_WIFI(headingtc, DEC); | ||
| + | calculateDir(dir, headingtc); | ||
| + | writeString_P_WIFI(" Direction: "); | ||
| + | writeString_WIFI(dir); | ||
| + | writeChar_WIFI('\n'); | ||
| + | #ifdef GET_TEMP_M | ||
| + | temperature_imu = (double) temperaturem / 8.0 + OFFSET_TEMP_M; | ||
| + | writeString_P_WIFI("Temperature [°]: "); | ||
| + | writeDouble_WIFI(temperature_imu, 7, 2); | ||
| + | writeChar_WIFI('\n'); | ||
| + | #endif | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("Heading "); | ||
| + | writeIntegerLCD(headingm, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | writeStringLCD(dir); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("X"); | ||
| + | writeIntegerLCD(x_axism, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 5); // line 2 pos 6 | ||
| + | writeStringLCD_P("Y"); | ||
| + | writeIntegerLCD(y_axism, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 10); // line 2 pos 11 | ||
| + | writeStringLCD_P("Z"); | ||
| + | writeIntegerLCD(z_axism, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | task_I2CTWI(); | ||
| + | #endif | ||
| + | setStopwatch1(0); | ||
| + | } | ||
| + | |||
| + | task_I2CTWI(); | ||
| + | } | ||
| + | |||
| + | return 0; | ||
| + | } | ||
| + | </pre> | ||
| + | |||
| + | ======Erklärung====== | ||
| + | |||
| + | =====GYRO Demo 1===== | ||
| + | Die [[RP6_Multi_IO_Projekt_-_Software#Demo_3|Demo für die Orientation Library]] zeigt als Ausgaben des 3D-Gyros nur die Werte der 3 Achsen. Mit dem GYRO ist natürlich viel mehr möglich. Diese 1. GYRO Demo gibt fortlaufend die Winkelgeschwindigkeiten auf dem LCD aus, mit denen der RP6 bewegt wird. | ||
| + | |||
| + | makefile: | ||
| + | <pre>... | ||
| + | TARGET = RP6M256_MultiIO_05_GYRO_01 | ||
| + | ... | ||
| + | SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256uart1.c | ||
| + | SRC += RP6M256_OrientationLib.c | ||
| + | ...</pre> | ||
| + | |||
| + | Datei: RP6M256_MultiIO_05_GYRO_01.c | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 MultiIO | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * In this example we show a fifth test for the MultiIO Project Board. | ||
| + | * It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU). | ||
| + | * | ||
| + | * The following sensors are used: | ||
| + | * - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS | ||
| + | * chipset | ||
| + | * - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG | ||
| + | * 2-axis magnetometer | ||
| + | * - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and | ||
| + | * the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer | ||
| + | * | ||
| + | * ############################################################################ | ||
| + | * The Robot does NOT move in this example! You can simply put it on a table | ||
| + | * next to your PC and you should connect it to the PC via the USB Interface! | ||
| + | * You should also connect to it via WIFI. | ||
| + | * ############################################################################ | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" // The RP6 M256 Library. | ||
| + | // Always needs to be included! | ||
| + | #include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library | ||
| + | #include "RP6M256uart1.h" // The RP6 M256 UART1 Library | ||
| + | #include <math.h> | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Include our new "RP6M256 Orientation library": | ||
| + | // (This is the library for accessing certain sensors connected to the | ||
| + | // MultiIO Project Board!) | ||
| + | |||
| + | #include "RP6M256_OrientationLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Defines: | ||
| + | |||
| + | //#define CALIBRATION | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Variables: | ||
| + | |||
| + | double xdpsg = 0.0; | ||
| + | double ydpsg = 0.0; | ||
| + | double zdpsg = 0.0; | ||
| + | double last_xg = 0.0; | ||
| + | double last_yg = 0.0; | ||
| + | double last_zg = 0.0; | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #define CTRL_REG5_FIFO_EN 0b01000000 // FIFO on | ||
| + | #define FIFO_CTRL_REG_BYPASS 0b00000000 // Bypass mode (default) | ||
| + | #define FIFO_CTRL_REG_FIFO 0b00100000 // FIFO mode | ||
| + | #define FIFO_CTRL_REG_STREAM 0b01000000 // Stream mode | ||
| + | #define FIFO_CTRL_REG_STREAM2FIFO 0b01100000 // Stream-to-FIFO mode | ||
| + | #define FIFO_CTRL_REG_BYPASS2STREAM 0b10000000 // Bypass-to-Stream mode | ||
| + | |||
| + | /** | ||
| + | * Call this once in order to use the 3D-Gyro | ||
| + | * with the FIFO in Stream mode. | ||
| + | * | ||
| + | * Hint: Interrupts or FIFO watermark infos are | ||
| + | * NOT used here! | ||
| + | * | ||
| + | */ | ||
| + | void L3GD20_initStreamMode(void) | ||
| + | { | ||
| + | // Choose Stream mode: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, FIFO_CTRL_REG, FIFO_CTRL_REG_STREAM); | ||
| + | mSleep(10); | ||
| + | // Activate FIFO: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG5, CTRL_REG5_FIFO_EN); | ||
| + | mSleep(10); | ||
| + | } | ||
| + | |||
| + | #define DPS_PER_DIGIT 0.00875 // @ +-250 dps (default) | ||
| + | //#define DPS_PER_DIGIT 0.0175 // @ +-500 dps | ||
| + | //#define DPS_PER_DIGIT 0.07 // @ +-2000 dps | ||
| + | |||
| + | /** | ||
| + | * This is the L3GD20 gyroscope task. | ||
| + | * | ||
| + | * The task stores the averaged dps values in | ||
| + | * the global double variables xdpsg, ydpsg, | ||
| + | * zdpsg. | ||
| + | * | ||
| + | * ATTENTION: Stopwatch 7 is used for the GYRO | ||
| + | * task! Please do not use this | ||
| + | * stopwatch elsewhere in your | ||
| + | * program! | ||
| + | * | ||
| + | * Hint: You must calibrate the gyro carefully | ||
| + | * before using this task! | ||
| + | * | ||
| + | */ | ||
| + | void task_GYRO(void) | ||
| + | { | ||
| + | uint8_t readBuf[6]; | ||
| + | |||
| + | if(getStopwatch7() > 100) // 100ms | ||
| + | { | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80)); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read X-/Y-/Z-axis | ||
| + | // xb = y: | ||
| + | x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L]; | ||
| + | // yb = -x: | ||
| + | y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L]; | ||
| + | y_axisg *= -1; | ||
| + | // zb = z: | ||
| + | z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L]; | ||
| + | normalizeL3GD20(); | ||
| + | xg *= DPS_PER_DIGIT; | ||
| + | yg *= DPS_PER_DIGIT; | ||
| + | zg *= DPS_PER_DIGIT; | ||
| + | xdpsg = (last_xg + xg) / 2; | ||
| + | ydpsg = (last_yg + yg) / 2; | ||
| + | zdpsg = (last_zg + zg) / 2; | ||
| + | last_xg = xg; | ||
| + | last_yg = yg; | ||
| + | last_zg = zg; | ||
| + | setStopwatch7(0); | ||
| + | } | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /** | ||
| + | * Write a floating point number to the LCD. | ||
| + | * | ||
| + | * Example: | ||
| + | * | ||
| + | * // Write a floating point number to the LCD (no exponent): | ||
| + | * writeDoubleLCD(1234567.890, 11, 3); | ||
| + | * | ||
| + | * The value of prec (precision) defines the number of decimal places. | ||
| + | * For 32 bit floating point variables (float, double ...) 6 is | ||
| + | * the max. value for prec (7 relevant digits). | ||
| + | * The value of width defines the overall number of characters in the | ||
| + | * floating point number including the decimal point. The number of | ||
| + | * pre-decimal positions is: (width - prec - 1). | ||
| + | */ | ||
| + | void writeDoubleLCD(double number, uint8_t width, uint8_t prec) | ||
| + | {char buffer[width + 1]; | ||
| + | dtostrf(number, width, prec, &buffer[0]); | ||
| + | writeStringLCD(&buffer[0]); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Error handler | ||
| + | |||
| + | /** | ||
| + | * This function gets called automatically if there was an I2C Error like | ||
| + | * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30). | ||
| + | * | ||
| + | */ | ||
| + | void I2C_transmissionError(uint8_t errorState) | ||
| + | { | ||
| + | writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x"); | ||
| + | writeInteger_WIFI(errorState, HEX); | ||
| + | writeChar_WIFI('\n'); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Main function - The program starts here: | ||
| + | |||
| + | int main(void) | ||
| + | { | ||
| + | initRP6M256(); // Always call this first! The Processor will not work | ||
| + | // correctly otherwise. | ||
| + | |||
| + | initLCD(); // Initialize the LC-Display (LCD) | ||
| + | // Always call this before using the LCD! | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | mSleep(500); | ||
| + | setLEDs(0b0000); | ||
| + | |||
| + | writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5 GYRO!\n"); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation | ||
| + | // with 100kHz SCL Frequency | ||
| + | |||
| + | |||
| + | // Register the event handler: | ||
| + | I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | |||
| + | // Write a text message to the LCD: | ||
| + | showScreenLCD("################", "################"); | ||
| + | mSleep(1500); | ||
| + | showScreenLCD("RP6v2-M256-WIFI ", "Example Program"); | ||
| + | mSleep(2500); | ||
| + | showScreenLCD("RP6M256 Multi IO", "Selftest 5 GYRO"); | ||
| + | mSleep(2500); | ||
| + | clearLCD(); | ||
| + | |||
| + | setLEDs(0b0000); | ||
| + | |||
| + | // --------------------------------------- | ||
| + | |||
| + | clearLCD(); | ||
| + | startStopwatch1(); | ||
| + | startStopwatch7(); // Used for GYRO task! | ||
| + | |||
| + | // IMPORTANT: | ||
| + | orientation_init(); // Orientation init! | ||
| + | |||
| + | L3GD20_initStreamMode(); // Gyro Stream mode init! | ||
| + | |||
| + | while(true) | ||
| + | { | ||
| + | #ifndef CALIBRATION | ||
| + | task_I2CTWI(); | ||
| + | task_GYRO(); | ||
| + | #endif | ||
| + | task_I2CTWI(); | ||
| + | if(getStopwatch1() > 500) // 500ms | ||
| + | { | ||
| + | #ifdef CALIBRATION | ||
| + | // GYRO calibration part: | ||
| + | readL3GD20(); // Get sensor values | ||
| + | //normalizeL3GD20(); | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("X"); | ||
| + | writeIntegerLCD(x_axisg, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(0, 8); // line 1 pos 9 | ||
| + | writeStringLCD_P("Y"); | ||
| + | writeIntegerLCD(y_axisg, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("Z"); | ||
| + | writeIntegerLCD(z_axisg, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | #else | ||
| + | // Display part: | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("X"); | ||
| + | writeDoubleLCD(xdpsg, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(0, 8); // line 1 pos 9 | ||
| + | writeStringLCD_P("Y"); | ||
| + | writeDoubleLCD(ydpsg, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("Z"); | ||
| + | writeDoubleLCD(zdpsg, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | #endif | ||
| + | setStopwatch1(0); | ||
| + | } | ||
| + | |||
| + | } | ||
| + | |||
| + | return 0; | ||
| + | } | ||
| + | </pre> | ||
| + | |||
| + | ======Erklärung====== | ||
| + | |||
| + | =====GYRO Demo 2===== | ||
| + | Die [[RP6_Multi_IO_Projekt_-_Software#Demo_3|Demo für die Orientation Library]] zeigt als Ausgaben des 3D-Gyros nur die Werte der 3 Achsen. Mit dem GYRO ist natürlich viel mehr möglich. Diese 2. GYRO Demo gibt fortlaufend die aufsummierten Winkel einer Drehung auf dem LCD aus. | ||
| + | |||
| + | In Ruhelage des RP6 sollten die Werte bei 0 bleiben oder nur ganz langsam über die Zeit "wegdriften". Dieses Driften ist einerseits eine (schlechte) Gyro-Eigenschaft, die hoch temperaturabhängig ist und sich nicht beeinflussen läßt. | ||
| + | Andererseits: Wenn die Werte schnell (innerhalb weniger Sekunden) wegdriften, dann ist die Kalibrierung nicht sorgfältig genug gemacht worden. Wenn man das nicht besser hinkriegt, kann man auch den Wert von OFFSET_ZERO (aktuell 75) weiter erhöhen. Besser ist aber IMMER die gute Kalibrierung, weil durch die Erhöhung von OFFSET_ZERO die "Empfindlichkeit" für sehr langsame Drehungen geringer wird. | ||
| + | |||
| + | '''Kalibrierung des GYRO mit der Demo 2''' | ||
| + | |||
| + | Man kann die Kalibrierung des Gyro auch gut mit dieser Demo 2 machen: | ||
| + | Wenn man in der Demo die "//" vor #define CALIBRATION wegmacht, kann man mit der Demo 2 die Rohwerte ausgeben. | ||
| + | |||
| + | 1. In RP6M256_Orientation.h: | ||
| + | a) #define IMU_9D aktivieren, die beiden anderen Definitionen (GPS, COMPASS_2D) | ||
| + | auskommentieren | ||
| + | b) #define GET_TEMP auskommentieren: //#define GET_TEMP | ||
| + | c) OFFSET_X, OFFSET_Y, OFFSET_Z auf 0 setzen | ||
| + | d) Datei speichern | ||
| + | |||
| + | 2. Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien | ||
| + | der Demo VORHER LÖSCHEN!) | ||
| + | |||
| + | 3. In Ruhelage des RP6 die Werte beobachten: Schwanken sie z.B. bei X zwischen | ||
| + | +20 und +140, also im Mittel um den Wert +80, dann in OFFSET_X -80 | ||
| + | eintragen. Das so für alle 3 Achsen machen. Noch ein Beispiel: Schwanken | ||
| + | die Werte bei Y zwischen -50 und +30, also im Mittel um -10, dann in | ||
| + | OFFSET_Y +10 eintragen. Am Ende RP6M256_Orientation.h speichern. | ||
| + | |||
| + | 4. In der Demo die "//" vor normalizeL3GD20(); wegmachen | ||
| + | |||
| + | 5. Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien | ||
| + | der Demo VORHER LÖSCHEN!) | ||
| + | |||
| + | 6. Ausgabewerte ansehen: Sie müßten jetzt um den Nullpunkt schwanken,- | ||
| + | jedenfalls viel besser um 0 gemittelt sein. Gibt es da noch konstante | ||
| + | Abweichungen, dann mit Punkt 3. weitermachen, dabei die neuen Offsets nur | ||
| + | zu den vorhandenen addieren, also nicht bei 0 wieder anfangen. (Dabei | ||
| + | bleibt die Zeile normalizeL3GD20(); immer mit drin!) Das so lange machen, | ||
| + | bis man zufrieden ist, dann Kalibrierung ENDE!!! | ||
| + | |||
| + | 7. Jetzt KANN es noch mit der Demo GYRO_02 ans "Feintuning" gehen: | ||
| + | a) In der Demo die "//" vor #define CALIBRATION wieder hinmachen (Messbetrieb, | ||
| + | keine Kalibrierung mehr!). | ||
| + | b) Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien | ||
| + | der Demo VORHER LÖSCHEN!) | ||
| + | c) Idealerweise bleiben in Ruhelage des RP6 jetzt alle Achsen dauerhaft bei | ||
| + | 0.0, dann Feintuning ENDE!!! | ||
| + | d) Ist das nicht der Fall und wandert eine Anzeige langsam immer weiter in den | ||
| + | negativen oder positiven Bereich, kann man in der RP6M256_Orientation.h | ||
| + | die OFFSET_X, _Y, _Z Werte vorsichtig korrigieren: Ich würde um max. +- 5 | ||
| + | korrigieren, die Datei speichern und bei 7.b) weiter machen. | ||
| + | |||
| + | Noch ein Hinweis für ganz Exakte: Man kann jetzt noch den Wert OFFSET_ZERO der Demo kleiner machen (z.B. auf 60 oder 50 reduzieren). Dann driften die schön kalibrierten Werte evtl. wieder weg und man kann bei 7.b) weiter machen. ENDE des Feintuning [;-)] unter folgenden Bedingungen (was eher zutrifft): | ||
| + | |||
| + | * Die Korrektur der OFFSET_X, _Y, _Z Werte erfolgt im Wechsel um + 1 oder - 1. | ||
| + | * OFFSET_ZERO ist kleiner als 50. | ||
| + | * Das Wegdriften der Winkel-Werte beginnt erst nach 1/2 Stunde. | ||
| + | * Das Feintuning dauert insgesamt länger als 3 Stunden. | ||
| + | * Die Maximalzahl der Schreibzyklen auf den Flash-Speicher des Microcontrollers ist überschritten. | ||
| + | * Man vernachlässigt seine sonstigen Aufgaben im Leben. | ||
| + | |||
| + | |||
| + | makefile: | ||
| + | <pre>... | ||
| + | TARGET = RP6M256_MultiIO_05_GYRO_02 | ||
| + | ... | ||
| + | SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256uart1.c | ||
| + | SRC += RP6M256_OrientationLib.c | ||
| + | ...</pre> | ||
| + | |||
| + | Datei: RP6M256_MultiIO_05_GYRO_02.c | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 MultiIO | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * In this example we show a fifth test for the MultiIO Project Board. | ||
| + | * It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU). | ||
| + | * | ||
| + | * The following sensors are used: | ||
| + | * - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS | ||
| + | * chipset | ||
| + | * - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG | ||
| + | * 2-axis magnetometer | ||
| + | * - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and | ||
| + | * the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer | ||
| + | * | ||
| + | * ############################################################################ | ||
| + | * The Robot does NOT move in this example! You can simply put it on a table | ||
| + | * next to your PC and you should connect it to the PC via the USB Interface! | ||
| + | * You should also connect to it via WIFI. | ||
| + | * ############################################################################ | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" // The RP6 M256 Library. | ||
| + | // Always needs to be included! | ||
| + | #include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library | ||
| + | #include "RP6M256uart1.h" // The RP6 M256 UART1 Library | ||
| + | #include <math.h> | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Include our new "RP6M256 Orientation library": | ||
| + | // (This is the library for accessing certain sensors connected to the | ||
| + | // MultiIO Project Board!) | ||
| + | |||
| + | #include "RP6M256_OrientationLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Defines: | ||
| + | |||
| + | //#define CALIBRATION | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Variables: | ||
| + | |||
| + | double xdg = 0.0; | ||
| + | double ydg = 0.0; | ||
| + | double zdg = 0.0; | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #define CTRL_REG5_FIFO_EN 0b01000000 // FIFO on | ||
| + | #define FIFO_CTRL_REG_BYPASS 0b00000000 // Bypass mode (default) | ||
| + | #define FIFO_CTRL_REG_FIFO 0b00100000 // FIFO mode | ||
| + | #define FIFO_CTRL_REG_STREAM 0b01000000 // Stream mode | ||
| + | #define FIFO_CTRL_REG_STREAM2FIFO 0b01100000 // Stream-to-FIFO mode | ||
| + | #define FIFO_CTRL_REG_BYPASS2STREAM 0b10000000 // Bypass-to-Stream mode | ||
| + | |||
| + | /** | ||
| + | * Call this once in order to use the 3D-Gyro | ||
| + | * with the FIFO in Stream mode. | ||
| + | * | ||
| + | * Hint: Interrupts or FIFO watermark infos are | ||
| + | * NOT used here! | ||
| + | * | ||
| + | */ | ||
| + | void L3GD20_initStreamMode(void) | ||
| + | { | ||
| + | // Choose Stream mode: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, FIFO_CTRL_REG, FIFO_CTRL_REG_STREAM); | ||
| + | mSleep(10); | ||
| + | // Activate FIFO: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG5, CTRL_REG5_FIFO_EN); | ||
| + | mSleep(10); | ||
| + | } | ||
| + | |||
| + | #define DPS_PER_DIGIT 0.00875 // @ +-250 dps (default) | ||
| + | //#define DPS_PER_DIGIT 0.0175 // @ +-500 dps | ||
| + | //#define DPS_PER_DIGIT 0.07 // @ +-2000 dps | ||
| + | #define OFFSET_ZERO 75 // Zero offset (of raw values) | ||
| + | #define D_CORR_FACTOR 1.065 // Corrigation factor | ||
| + | #define GYRO_INTERVAL 50 // Measuring interval [ms] | ||
| + | |||
| + | /** | ||
| + | * This is the L3GD20 gyroscope task. | ||
| + | * | ||
| + | * The task stores the cumulated angle values in | ||
| + | * the global double variables xdg, ydg, zdg. | ||
| + | * | ||
| + | * ATTENTION: Stopwatch 7 is used for the GYRO | ||
| + | * task! Please do not use this | ||
| + | * stopwatch elsewhere in your | ||
| + | * program! | ||
| + | * | ||
| + | * Hint: You must calibrate the gyro carefully | ||
| + | * before using this task! | ||
| + | * | ||
| + | */ | ||
| + | void task_GYRO(void) | ||
| + | { | ||
| + | uint8_t readBuf[6]; | ||
| + | |||
| + | if(getStopwatch7() > GYRO_INTERVAL) // GYRO_INTERVAL [ms] | ||
| + | { | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80)); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read X-/Y-/Z-axis | ||
| + | // xb = y: | ||
| + | x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L]; | ||
| + | // yb = -x: | ||
| + | y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L]; | ||
| + | y_axisg *= -1; | ||
| + | // zb = z: | ||
| + | z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L]; | ||
| + | normalizeL3GD20(); | ||
| + | xg *= DPS_PER_DIGIT; // Calculate dps | ||
| + | yg *= DPS_PER_DIGIT; | ||
| + | zg *= DPS_PER_DIGIT; | ||
| + | xg /= ((double) 1000 / GYRO_INTERVAL); // Angle per interval | ||
| + | yg /= ((double) 1000 / GYRO_INTERVAL); | ||
| + | zg /= ((double) 1000 / GYRO_INTERVAL); | ||
| + | if (abs(x_axisg) > OFFSET_ZERO) // Don't cumulate 0 dps values | ||
| + | xdg += (xg * D_CORR_FACTOR); // Cumulated angle values [°] | ||
| + | if (abs(y_axisg) > OFFSET_ZERO) | ||
| + | ydg += (yg * D_CORR_FACTOR); | ||
| + | if (abs(z_axisg) > OFFSET_ZERO) | ||
| + | zdg += (zg * D_CORR_FACTOR); | ||
| + | setStopwatch7(0); | ||
| + | } | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function resets the cumulated angle | ||
| + | * values [°] stored in the global double | ||
| + | * variables xdg, ydg, zdg by the L3GD20 | ||
| + | * gyroscope task. | ||
| + | * | ||
| + | */ | ||
| + | void resetGYRO(void) | ||
| + | { | ||
| + | xdg = 0.0; | ||
| + | ydg = 0.0; | ||
| + | zdg = 0.0; | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /** | ||
| + | * Write a floating point number to the LCD. | ||
| + | * | ||
| + | * Example: | ||
| + | * | ||
| + | * // Write a floating point number to the LCD (no exponent): | ||
| + | * writeDoubleLCD(1234567.890, 11, 3); | ||
| + | * | ||
| + | * The value of prec (precision) defines the number of decimal places. | ||
| + | * For 32 bit floating point variables (float, double ...) 6 is | ||
| + | * the max. value for prec (7 relevant digits). | ||
| + | * The value of width defines the overall number of characters in the | ||
| + | * floating point number including the decimal point. The number of | ||
| + | * pre-decimal positions is: (width - prec - 1). | ||
| + | */ | ||
| + | void writeDoubleLCD(double number, uint8_t width, uint8_t prec) | ||
| + | {char buffer[width + 1]; | ||
| + | dtostrf(number, width, prec, &buffer[0]); | ||
| + | writeStringLCD(&buffer[0]); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Error handler | ||
| + | |||
| + | /** | ||
| + | * This function gets called automatically if there was an I2C Error like | ||
| + | * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30). | ||
| + | * | ||
| + | */ | ||
| + | void I2C_transmissionError(uint8_t errorState) | ||
| + | { | ||
| + | writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x"); | ||
| + | writeInteger_WIFI(errorState, HEX); | ||
| + | writeChar_WIFI('\n'); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Main function - The program starts here: | ||
| + | |||
| + | int main(void) | ||
| + | { | ||
| + | initRP6M256(); // Always call this first! The Processor will not work | ||
| + | // correctly otherwise. | ||
| + | |||
| + | initLCD(); // Initialize the LC-Display (LCD) | ||
| + | // Always call this before using the LCD! | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | mSleep(500); | ||
| + | setLEDs(0b0000); | ||
| + | |||
| + | writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5 GYRO!\n"); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation | ||
| + | // with 100kHz SCL Frequency | ||
| + | |||
| + | |||
| + | // Register the event handler: | ||
| + | I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | |||
| + | // Write a text message to the LCD: | ||
| + | showScreenLCD("################", "################"); | ||
| + | mSleep(1500); | ||
| + | showScreenLCD("RP6v2-M256-WIFI ", "Example Program"); | ||
| + | mSleep(2500); | ||
| + | showScreenLCD("RP6M256 Multi IO", "Selftest 5 GYRO"); | ||
| + | mSleep(2500); | ||
| + | clearLCD(); | ||
| + | |||
| + | setLEDs(0b0000); | ||
| + | |||
| + | // --------------------------------------- | ||
| + | |||
| + | clearLCD(); | ||
| + | startStopwatch1(); | ||
| + | startStopwatch7(); // Used for GYRO task! | ||
| + | |||
| + | // IMPORTANT: | ||
| + | orientation_init(); // Orientation init! | ||
| + | |||
| + | L3GD20_initStreamMode(); // Gyro Stream mode init! | ||
| + | resetGYRO(); // Reset GYRO! | ||
| + | |||
| + | while(true) | ||
| + | { | ||
| + | #ifndef CALIBRATION | ||
| + | task_I2CTWI(); | ||
| + | task_GYRO(); | ||
| + | #endif | ||
| + | task_I2CTWI(); | ||
| + | if(getStopwatch1() > 500) // 500ms | ||
| + | { | ||
| + | #ifdef CALIBRATION | ||
| + | // GYRO calibration part: | ||
| + | readL3GD20(); // Get sensor values | ||
| + | //normalizeL3GD20(); | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("X"); | ||
| + | writeIntegerLCD(x_axisg, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(0, 8); // line 1 pos 9 | ||
| + | writeStringLCD_P("Y"); | ||
| + | writeIntegerLCD(y_axisg, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("Z"); | ||
| + | writeIntegerLCD(z_axisg, DEC); | ||
| + | writeStringLCD_P(" "); | ||
| + | #else | ||
| + | // Display part: | ||
| + | setCursorPosLCD(0, 0); // line 1 | ||
| + | writeStringLCD_P("X"); | ||
| + | writeDoubleLCD(xdg, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(0, 8); // line 1 pos 9 | ||
| + | writeStringLCD_P("Y"); | ||
| + | writeDoubleLCD(ydg, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | setCursorPosLCD(1, 0); // line 2 | ||
| + | writeStringLCD_P("Z"); | ||
| + | writeDoubleLCD(zdg, 6, 1); | ||
| + | writeStringLCD_P(" "); | ||
| + | #endif | ||
| + | setStopwatch1(0); | ||
| + | } | ||
| + | |||
| + | } | ||
| + | |||
| + | return 0; | ||
| + | }</pre> | ||
| + | |||
======Erklärung====== | ======Erklärung====== | ||
====Library für die Messung von Umweltdaten==== | ====Library für die Messung von Umweltdaten==== | ||
| + | Zur Messung von Umweltdaten können folgende drei Sensoren genutzt und direkt an die Multi IO Projekt Platine angeschlossen werden: | ||
| + | * Zwei LDR A9060 (Helligkeit) | ||
| + | * HYT-221 (Feuchte, Temperatur) | ||
| + | * BMP085 (Luftdruck, Temperatur) | ||
| − | ... | + | Bringt man z.B. den HYT-221 und einen LDR A9060 draußen an, dann kann man mit den drei Außenwerten (Feuchte, Temperatur, Helligkeit) und dem innen betriebenen BMP085 und dem zweiten LDR (Luftdruck, Innentemperatur, Innenhelligkeit) schon eine recht gute "Wetterstation" darstellen. |
| + | |||
| + | Diese [[Avr-gcc]] Library für das Multi IO Projekt Board (= "MultiIO") geht von folgenden Voraussetzungen aus: | ||
| + | * Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt. | ||
| + | * Die M256 ist der I2C-Bus Master. | ||
| + | * Die I2C-Bus Geschwindigkeit beträgt 100 kHz. | ||
| + | * Alle Jumper auf der MultiIO sind in ihrer Standardstellung (1). | ||
| + | * Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden. | ||
| + | * Der Wannenstecker ADC_M256 der MultiIO ist mit dem Wannenstecker ADC_IO1 der M256 1:1 verbunden (2). | ||
| + | |||
| + | Zu (1): Siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]! | ||
| + | Zu (2): Nur nötig, wenn die Helligkeitssensoren benutzt werden sollen! | ||
| + | |||
| + | =====3,3V-Stromversorgung===== | ||
| + | Der I2C-Luftdrucksensor BMP085 wird mit einer Versorgungsspannung von 3,3V und an einem 3,3V-I2C-Bus betrieben. Diese Voraussetzungen sind gegeben, wenn der Sensor an seinen 6-poligen Anschluß-Stecker "Luftdruck" auf der MultiIO angeschlossen wird. Allerdings muss noch die 3,3V-Stromversorgung der MultiIO eingeschaltet werden. Dazu sind zwei zusätzliche Jumper zu setzen, siehe folgende Abbildung: | ||
| + | |||
| + | [[Bild:MultiIO_J_3V3-Stromversorgung.PNG|Jumper der 3,3V-Stromversorgung]] | ||
| + | |||
| + | =====Pullup-Widerstände===== | ||
| + | Auf dem I2C-Luftdrucksensormodul mit dem BMP085 Sensor sind Pullup-Widerstände (R1, R2) von je 4,7 kOhm an SCL und SDA vorgesehen. Für die MultiIO Platine sind diese Widerstände nicht erforderlich und müssen deaktiviert werden. Dazu gibt es bei der "BMP085 Breakout" Platine Version v14 einen Jumper SJ1. Dieser Jumper muss auf beiden Seiten aufgetrennt werden, indem das Lötzinn dort entfernt wird. Bei älteren Versionen der Platine ohne den Jumper SJ1 müssen die beiden Pullup-Widerstände ausgelötet werden. | ||
| + | |||
| + | |||
| + | Die '''Library''' (Software-Bibliothek) besteht aus drei Teilen: | ||
| + | * Dem [[RP6_Multi_IO_Projekt_-_Software#Configuration Header_3|Configuration Header]] -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | ||
| + | * Dem [[RP6_Multi_IO_Projekt_-_Software#Library Header_5|Library Header]] -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library. | ||
| + | * Der [[RP6_Multi_IO_Projekt_-_Software#Library Source_5|Library Source]] -> Das ist die eigentliche Library. | ||
| + | |||
| + | Zusätzlich wird auch der [[RP6_Multi_IO_Projekt_-_Software#Configuration Header|MultiIO Configuration Header]] eingebunden. Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | ||
| + | |||
| + | =====Configuration Header===== | ||
| + | Datei RP6M256_Weather.h: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 Weather Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * Configuration header file for new Weather library. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | #ifndef RP6M256_WEATHER_H | ||
| + | #define RP6M256_WEATHER_H | ||
| + | |||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // The following sensors are used: | ||
| + | // - Two LDR A9060 brightness sensors (CONRAD No. 145475) | ||
| + | // - HYT-221 humidity & temperature module (CONRAD No. 505671) | ||
| + | // - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085 | ||
| + | // air pressure sensor | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | // Define the devices that are connected to the MultiIO Project Board here! | ||
| + | // Make a comment of a line belonging to a sensor that is NOT attached to the | ||
| + | // MultiIO Board! | ||
| + | #define BRIGHTNESS_SENSORS // Brightness sensors (LDRs) are used | ||
| + | #define HUMIDITY_TEMP_SENSOR // Humidity & temper. sensor is used | ||
| + | #define AIRPRESSURE_SENSOR // Airpressure sensor is used | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Weather station's data: | ||
| + | #define ALTITUDE 156 // Weather station's altitude [m] | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Brightness sensors (LDRs): | ||
| + | |||
| + | #define LDR1_ERROR 0 // LDR1 error (-150 ... +150 lx) | ||
| + | #define LDR2_ERROR 0 // LDR2 error (-150 ... +150 lx) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Humidity & temperature sensor (HYT-221): | ||
| + | |||
| + | #define HYT_RH_ERROR 0.0 // RH error (-2.0 ... +2.0 %) | ||
| + | #define HYT_T_ERROR 0.0 // Temp. error (-1.0 ... +1.0 °C) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Airpressure sensor (BMP085): | ||
| + | |||
| + | #define BMP_P_ERROR 0 // P error (-300 ... +300 Pa) | ||
| + | #define BMP_T_ERROR 0 // Temp. error (-20 ... +20 / 10 °C) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #endif | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * | ||
| + | * ---> changes are documented in the file "RP6M256_WeatherLib.c" | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
=====Library Header===== | =====Library Header===== | ||
| + | Datei RP6M256_WeatherLib.h: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 Weather Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * Header file for new Weather library. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | #ifndef RP6M256_WEATHERLIB_H | ||
| + | #define RP6M256_WEATHERLIB_H | ||
| + | |||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // The following sensors are used: | ||
| + | // - Two LDR A9060 brightness sensors (CONRAD No. 145475) | ||
| + | // - HYT-221 humidity & temperature module (CONRAD No. 505671) | ||
| + | // - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085 | ||
| + | // air pressure sensor | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" | ||
| + | #include "RP6I2CmasterTWI.h" | ||
| + | #include "RP6M256_Weather.h" | ||
| + | #include "RP6M256_MultiIO.h" | ||
| + | #include "avr/pgmspace.h" | ||
| + | #include "math.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Defines: | ||
| + | |||
| + | #ifndef LEFT | ||
| + | #define LEFT 2 | ||
| + | #endif | ||
| + | #ifndef RIGHT | ||
| + | #define RIGHT 3 | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Brightness sensors (LDRs): | ||
| + | |||
| + | #ifdef BRIGHTNESS_SENSORS | ||
| + | // Flash Constants: | ||
| + | const uint16_t pgm_ldr_adc[17]; | ||
| + | const int16_t pgm_ldr_lux[17]; | ||
| + | // Variables: | ||
| + | extern uint16_t adcldr1_l; // Left LDR1 sensor ADC value | ||
| + | extern uint16_t adcldr2_r; // Right LDR2 sensor ADC value | ||
| + | extern int16_t luxldr1_l; // Left LDR1 Lux value [lx] | ||
| + | extern int16_t luxldr2_r; // Right LDR2 Lux value [lx] | ||
| + | |||
| + | uint16_t getBRIGHTNESSSensor(uint8_t side); | ||
| + | int16_t calculateBRIGHTNESS(uint8_t side); | ||
| + | int16_t measureBRIGHTNESS(uint8_t side); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Humidity & temperature sensor (HYT-221): | ||
| + | |||
| + | // HYT-221 sensor: | ||
| + | #define I2C_MULTIIO_HYT221_ADR 0x50 // Default | ||
| + | |||
| + | // Status and data bitmasks: | ||
| + | #define HYT221_STATUS_STALE 64 // No new value | ||
| + | #define HYT221_STATUS_CMODE 128 // Command mode | ||
| + | #define HYT221_RH_H_MASK 63 // RH data high byte (6 bit) | ||
| + | |||
| + | // Command mode settings (see "I2C_Address change-HYT1.1.pdf"!): | ||
| + | #define CMD_START_CMODE 0x0a // Start command-mode | ||
| + | #define CMD_READ_CONFIG_PARAMS 0x1c // Read config. parameters | ||
| + | #define CMD_WRITE_CONFIG_PARAMS 0x5c // Write config. parameters | ||
| + | #define CMD_FINISH_CMODE 0x80 // Start normal-mode | ||
| + | |||
| + | #ifdef HUMIDITY_TEMP_SENSOR | ||
| + | extern uint16_t humidity; // Humidity raw value | ||
| + | extern uint16_t h_temperature; // Temperature raw value | ||
| + | extern double rh; // Humidity value [%] | ||
| + | extern double ht; // Temperature value [°C] | ||
| + | |||
| + | void HUMIDITY_TEMP_init(void); | ||
| + | void HYT221_mr(void); | ||
| + | void HYT221_read(void); | ||
| + | double HYT221_calculate(void); | ||
| + | #define getRHumidity() (rh) | ||
| + | #define getHTemperature() (ht) | ||
| + | double HYT221_measure(void); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Airpressure sensor (BMP085): | ||
| + | |||
| + | // BMP085 sensor: | ||
| + | #define I2C_MULTIIO_BMP085_ADR 0xee // Default | ||
| + | |||
| + | // BMP085 registers: | ||
| + | #define BMP085_AC1 0xaa // EEPROM registers (16 bit) | ||
| + | #define BMP085_AC2 0xac | ||
| + | #define BMP085_AC3 0xae | ||
| + | #define BMP085_AC4 0xb0 | ||
| + | #define BMP085_AC5 0xb2 | ||
| + | #define BMP085_AC6 0xb4 | ||
| + | #define BMP085_B1 0xb6 | ||
| + | #define BMP085_B2 0xb8 | ||
| + | #define BMP085_MB 0xba | ||
| + | #define BMP085_MC 0xbc | ||
| + | #define BMP085_MD 0xbe | ||
| + | #define BMP085_CHIP_ID 0xd0 // Other registers (8 bit) | ||
| + | #define BMP085_VERSION 0xd1 | ||
| + | #define BMP085_SOFT_RESET 0xe0 | ||
| + | #define BMP085_CONTROL 0xf4 // Control register | ||
| + | #define BMP085_DATA_MSB 0xf6 // Data registers | ||
| + | #define BMP085_DATA_LSB 0xf7 | ||
| + | #define BMP085_DATA_XLSB 0xf8 | ||
| + | |||
| + | // BMP085 control register settings: | ||
| + | #define CONTROL_TEMPERATURE 0x2e // Measure temperature | ||
| + | #define CONTROL_PRESSURE_OSRS0 0x34 // Measure pressure mode 0 | ||
| + | #define CONTROL_PRESSURE_OSRS1 0x74 // Measure pressure mode 1 | ||
| + | #define CONTROL_PRESSURE_OSRS2 0xb4 // Measure pressure mode 2 | ||
| + | #define CONTROL_PRESSURE_OSRS3 0xf4 // Measure pressure mode 3 | ||
| + | |||
| + | // Airpressure reducing formula constants: | ||
| + | #define TEMP_ZERO 273.15 // 273.15 K = 0 °C | ||
| + | |||
| + | #ifdef AIRPRESSURE_SENSOR | ||
| + | extern int16_t ac1; // EEPROM calibration data | ||
| + | extern int16_t ac2; | ||
| + | extern int16_t ac3; | ||
| + | extern uint16_t ac4; | ||
| + | extern uint16_t ac5; | ||
| + | extern uint16_t ac6; | ||
| + | extern int16_t b1; | ||
| + | extern int16_t b2; | ||
| + | extern int16_t mb; | ||
| + | extern int16_t mc; | ||
| + | extern int16_t md; | ||
| + | extern int32_t ut; // Temperature raw value | ||
| + | extern uint8_t oss; // Mode (0..3) | ||
| + | extern int32_t up; // Pressure raw value | ||
| + | extern int32_t t; // Temperature [/ 10 °C] | ||
| + | extern int32_t p; // Pressure [Pa] | ||
| + | extern int32_t p0; // Pressure at sea level [Pa] | ||
| + | extern int16_t alt; // Altitude [m] | ||
| + | |||
| + | void AIRPRESSURE_init(void); | ||
| + | void BMP085_read(void); | ||
| + | int32_t BMP085_calculate(void); | ||
| + | #define getPressure() (p) | ||
| + | #define getTemperature() (t) | ||
| + | int32_t BMP085_measure(void); | ||
| + | int32_t BMP085_reducePressure(int32_t p, uint16_t alt, int32_t t); | ||
| + | int16_t BMP085_getAltitude(int32_t p, int32_t p0, int16_t offset); | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Weather library initialisation: | ||
| + | |||
| + | void weather_init(void); | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | #endif | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * | ||
| + | * ---> changes are documented in the file "RP6M256_WeatherLib.c" | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
| + | |||
=====Library Source===== | =====Library Source===== | ||
| + | Datei RP6M256_WeatherLib.c: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 Weather Library | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * | ||
| + | * This is our new Library that contains basic routines and functions for | ||
| + | * accessing certain sensors connected to the MultiIO Project Board. | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // The following sensors are used: | ||
| + | // - Two LDR A9060 brightness sensors (CONRAD No. 145475) | ||
| + | // - HYT-221 humidity & temperature module (CONRAD No. 505671) | ||
| + | // - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085 | ||
| + | // air pressure sensor | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256_WeatherLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Variables: | ||
| + | |||
| + | uint8_t registerBuf[5]; | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Brightness sensors (LDRs): | ||
| + | |||
| + | #ifdef BRIGHTNESS_SENSORS | ||
| + | // Flash Constants: | ||
| + | const uint16_t pgm_ldr_adc[] PROGMEM = { | ||
| + | 0, 667, 845, 946, 954, 965, 971, 980, 994, | ||
| + | 1002, 1010, 1013, 1015, 1017, 1019, 1021, 1023}; | ||
| + | const int16_t pgm_ldr_lux[] PROGMEM = { | ||
| + | -32767, 0, 50, 100, 150, 200, 250, 300, 400, | ||
| + | 500, 750, 1000, 1500, 2000, 2500, 3000, 32768}; | ||
| + | // Variables: | ||
| + | uint16_t adcldr1_l; // Left LDR1 sensor ADC value | ||
| + | uint16_t adcldr2_r; // Right LDR2 sensor ADC value | ||
| + | |||
| + | /** | ||
| + | * This function reads and returns the ADC value | ||
| + | * of the left or right brightness (LDR) sensor. | ||
| + | * The value is also stored in adcldr1_l or | ||
| + | * adcldr2_r. | ||
| + | * | ||
| + | * Input: side -> 2 (LEFT) = Left LDR1 sensor | ||
| + | * 3 (RIGHT) = Right LDR2 sensor | ||
| + | * | ||
| + | */ | ||
| + | uint16_t getBRIGHTNESSSensor(uint8_t side) | ||
| + | { | ||
| + | if (side == LEFT) { | ||
| + | adcldr1_l = readADC(ADC_MULTIIO_LDR1); | ||
| + | return adcldr1_l; | ||
| + | } | ||
| + | if (side == RIGHT) { | ||
| + | adcldr2_r = readADC(ADC_MULTIIO_LDR2); | ||
| + | return adcldr2_r; | ||
| + | } | ||
| + | return 0; | ||
| + | } | ||
| + | |||
| + | int16_t luxldr1_l; // Left LDR1 Lux value [lx] | ||
| + | int16_t luxldr2_r; // Right LDR2 Lux value [lx] | ||
| + | |||
| + | /** | ||
| + | * Calculates and returns the left or right LDR | ||
| + | * sensor light intensity value [lx] by using | ||
| + | * the data read from the LDR sensor with the | ||
| + | * function getBRIGHTNESSSensor(). | ||
| + | * | ||
| + | * Input: side -> 2 (LEFT) = Left LDR1 sensor | ||
| + | * 3 (RIGHT) = Right LDR2 sensor | ||
| + | * | ||
| + | * Hint: - The lookup table pgm_ldr_adc and | ||
| + | * pgm_ldr_lux is adapted to the | ||
| + | * PerkinElmer LDR A9060_A9013 sensor | ||
| + | * ONLY (R41, R42 = 68 kOhm)! | ||
| + | * | ||
| + | */ | ||
| + | int16_t calculateBRIGHTNESS(uint8_t side) | ||
| + | { | ||
| + | uint8_t i; | ||
| + | int16_t lux_h, lux_l, vside; | ||
| + | uint16_t adc_h, adc_l; | ||
| + | double dvside; | ||
| + | |||
| + | if (side == LEFT) { | ||
| + | for (i = 1; i < 17; i++) { | ||
| + | adc_h = pgm_read_word(&pgm_ldr_adc[i]); | ||
| + | if (adcldr1_l <= adc_h) { | ||
| + | break; | ||
| + | } | ||
| + | } | ||
| + | adc_l = pgm_read_word(&pgm_ldr_adc[i - 1]); | ||
| + | lux_h = pgm_read_word(&pgm_ldr_lux[i]); | ||
| + | lux_l = pgm_read_word(&pgm_ldr_lux[i - 1]); | ||
| + | dvside = ((double) adcldr1_l - adc_l) / ((double) adc_h - adc_l) | ||
| + | * ((double) lux_h - lux_l) + (double) lux_l; | ||
| + | vside = dvside; | ||
| + | vside += LDR1_ERROR; | ||
| + | return vside; | ||
| + | } | ||
| + | if (side == RIGHT) { | ||
| + | for (i = 1; i < 17; i++) { | ||
| + | adc_h = pgm_read_word(&pgm_ldr_adc[i]); | ||
| + | if (adcldr2_r <= adc_h) { | ||
| + | break; | ||
| + | } | ||
| + | } | ||
| + | adc_l = pgm_read_word(&pgm_ldr_adc[i - 1]); | ||
| + | lux_h = pgm_read_word(&pgm_ldr_lux[i]); | ||
| + | lux_l = pgm_read_word(&pgm_ldr_lux[i - 1]); | ||
| + | dvside = ((double) adcldr2_r - adc_l) / ((double) adc_h - adc_l) | ||
| + | * ((double) lux_h - lux_l) + (double) lux_l; | ||
| + | vside = dvside; | ||
| + | vside += LDR2_ERROR; | ||
| + | return vside; | ||
| + | } | ||
| + | return 0; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Measures and returns the left or right LDR | ||
| + | * sensor light intensity value [lx]. | ||
| + | * The ADC value of the LDR sensor is also | ||
| + | * stored in adcldr1_l or adcldr2_r. | ||
| + | * | ||
| + | * Input: side -> 2 (LEFT) = Left LDR1 sensor | ||
| + | * 3 (RIGHT) = Right LDR2 sensor | ||
| + | * | ||
| + | */ | ||
| + | int16_t measureBRIGHTNESS(uint8_t side) | ||
| + | { | ||
| + | uint16_t tmp; | ||
| + | |||
| + | tmp = getBRIGHTNESSSensor(side); | ||
| + | return (calculateBRIGHTNESS(side)); // Result in [lx] | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Humidity & temperature sensor (HYT-221): | ||
| + | |||
| + | #ifdef HUMIDITY_TEMP_SENSOR | ||
| + | /** | ||
| + | * Call this once before using the humidity | ||
| + | * & temperature sensor. | ||
| + | * | ||
| + | */ | ||
| + | void HUMIDITY_TEMP_init(void) | ||
| + | { | ||
| + | // Nothing to do here in this software version ... | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Measuring request (MR) command. This function | ||
| + | * starts a measurement. The result is NOT read | ||
| + | * by this function (see HYT221_read()!). | ||
| + | * | ||
| + | */ | ||
| + | void HYT221_mr(void) | ||
| + | { | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_HYT221_ADR, 0); | ||
| + | } | ||
| + | |||
| + | uint16_t humidity; // Humidity raw value | ||
| + | uint16_t h_temperature; // Temperature raw value | ||
| + | |||
| + | /** | ||
| + | * Data fetch (DF) command. This function reads | ||
| + | * the humidity and temperature raw values from | ||
| + | * the humidity & temperatur sensor and stores | ||
| + | * them in the global variables humidity and | ||
| + | * h_temperature. | ||
| + | * | ||
| + | * Hints: - Each measurement has to be started | ||
| + | * with the function HYT_mr() before! | ||
| + | * - If the Stale bit (bit 14 of the RH | ||
| + | * data high byte) is set, the data are | ||
| + | * NOT new data and have already been | ||
| + | * read. The Stale bit is not used in | ||
| + | * this function. | ||
| + | * - You have to make a pause between | ||
| + | * starting the measurement (HYT_mr()!) | ||
| + | * and reading the result with this | ||
| + | * function OR you have to poll the | ||
| + | * Stale bit (0 = new data). | ||
| + | * | ||
| + | */ | ||
| + | void HYT221_read(void) | ||
| + | { | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_HYT221_ADR, registerBuf, 4); | ||
| + | humidity = ((registerBuf[0] & HYT221_RH_H_MASK) << 8) | registerBuf[1]; | ||
| + | h_temperature = ((registerBuf[2] << 8) | registerBuf[3]) >> 2; | ||
| + | } | ||
| + | |||
| + | double rh; // Humidity value [%] | ||
| + | double ht; // Temperature value [°C] | ||
| + | |||
| + | /** | ||
| + | * This function calculates the humidity and | ||
| + | * temperature values by using the sensor raw | ||
| + | * values humidity and h_temperature (see | ||
| + | * function HYT_read()!). The humidity value | ||
| + | * will be returned and stored in the global | ||
| + | * double variable rh. The temperature [°C] is | ||
| + | * stored in the global double variable ht. | ||
| + | * | ||
| + | * Output: Humidity [%] | ||
| + | * | ||
| + | */ | ||
| + | double HYT221_calculate(void) | ||
| + | { | ||
| + | rh = (double) humidity * 100.0 / 16383.0; | ||
| + | rh += HYT_RH_ERROR; | ||
| + | ht = (double) h_temperature * 165.0 / 16383.0 - 40.0; | ||
| + | ht += HYT_T_ERROR; | ||
| + | return rh; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Measures and returns the humidity [%] value. | ||
| + | * The temperature [°C] is stored in the global | ||
| + | * double variable ht. | ||
| + | * The humidity and temperature raw values are | ||
| + | * also stored in the global variables humidity | ||
| + | * and h_temperature. | ||
| + | * This function is BLOCKING for about 100 ms! | ||
| + | * If you don't want a blocking measurement, you | ||
| + | * have to write your own function with a non | ||
| + | * blocking pause between starting measurement | ||
| + | * and reading the result. | ||
| + | * | ||
| + | * Output: Humidity [%] | ||
| + | * | ||
| + | */ | ||
| + | double HYT221_measure(void) | ||
| + | { | ||
| + | HYT221_mr(); | ||
| + | mSleep(100); | ||
| + | HYT221_read(); | ||
| + | return (HYT221_calculate()); // Result in [%] | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Airpressure sensor (BMP085): | ||
| + | |||
| + | #ifdef AIRPRESSURE_SENSOR | ||
| + | int16_t ac1; // EEPROM calibration data | ||
| + | int16_t ac2; | ||
| + | int16_t ac3; | ||
| + | uint16_t ac4; | ||
| + | uint16_t ac5; | ||
| + | uint16_t ac6; | ||
| + | int16_t b1; | ||
| + | int16_t b2; | ||
| + | int16_t mb; | ||
| + | int16_t mc; | ||
| + | int16_t md; | ||
| + | int32_t ut; // Temperature raw value | ||
| + | uint8_t oss = 1; // Default: standard mode | ||
| + | uint8_t control_pressure_osrs; // Control register mode setting | ||
| + | int32_t up; // Pressure raw value | ||
| + | int32_t t; // Temperature [/ 10 °C] | ||
| + | int32_t p; // Pressure [Pa] | ||
| + | int32_t p0; // Pressure at sea level [Pa] | ||
| + | int16_t alt; // Altitude [m] | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the airpressure | ||
| + | * sensor. | ||
| + | * | ||
| + | */ | ||
| + | void AIRPRESSURE_init(void) | ||
| + | { | ||
| + | // Mode: | ||
| + | if (oss > 3) oss = 1; | ||
| + | control_pressure_osrs = (oss << 6) | CONTROL_PRESSURE_OSRS0; | ||
| + | // Calibration (EEPROM) data: | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC1); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ac1 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC2); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ac2 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC3); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ac3 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC4); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ac4 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC5); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ac5 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC6); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ac6 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_B1); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | b1 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_B2); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | b2 = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_MB); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | mb = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_MC); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | mc = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_MD); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | md = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function reads the pressure and | ||
| + | * temperature raw values from the airpressure | ||
| + | * sensor and stores them in the global long | ||
| + | * variables ut and up. | ||
| + | * | ||
| + | */ | ||
| + | void BMP085_read(void) | ||
| + | { | ||
| + | // Temperature: | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_BMP085_ADR, BMP085_CONTROL, CONTROL_TEMPERATURE); | ||
| + | mSleep(10); | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_DATA_MSB); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2); | ||
| + | ut = (registerBuf[0] << 8) | registerBuf[1]; | ||
| + | // Pressure: | ||
| + | control_pressure_osrs = (oss << 6) | CONTROL_PRESSURE_OSRS0; | ||
| + | I2CTWI_transmit2Bytes(I2C_MULTIIO_BMP085_ADR, BMP085_CONTROL, control_pressure_osrs); | ||
| + | mSleep(30); | ||
| + | I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_DATA_MSB); | ||
| + | I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 3); | ||
| + | up = (((uint32_t) registerBuf[0] << 16) | ||
| + | | ((uint32_t) registerBuf[1] << 8) | ||
| + | | ((uint32_t) registerBuf[2])) >> (8 - oss); | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * This function calculates the pressure and | ||
| + | * temperature values by using the sensor raw | ||
| + | * values ut, up (see function BMP085_read()!) | ||
| + | * and the EEPROM calibration data values ac1, | ||
| + | * ac2, ac3, ac4, ac5, ac6, b1, b2, mb, mc, md | ||
| + | * (see function AIRPRESSURE_init()!). The | ||
| + | * airpressure [Pa] value will be returned and | ||
| + | * stored in the global long variable p. The | ||
| + | * temperature [°C] is stored in the global long | ||
| + | * variable t. | ||
| + | * | ||
| + | * Output: Airpressure [Pa] | ||
| + | * | ||
| + | */ | ||
| + | int32_t BMP085_calculate(void) | ||
| + | { | ||
| + | int32_t x1, x2, b5, b6, x3, b3; | ||
| + | uint32_t b4, b7; | ||
| + | // Temperature: | ||
| + | x1 = ((int32_t) ut - ac6) * ac5 >> 15; | ||
| + | x2 = ((int32_t) mc << 11) / (x1 + md); | ||
| + | b5 = x1 + x2; | ||
| + | t = (b5 + 8) >> 4; | ||
| + | t += BMP_T_ERROR; | ||
| + | // Pressure: | ||
| + | b6 = b5 - 4000; | ||
| + | x1 = (b2 * (b6 * b6 >> 12)) >> 11; | ||
| + | x2 = ac2 * b6 >> 11; | ||
| + | x3 = x1 + x2; | ||
| + | b3 = ((((int32_t) ac1 * 4 + x3) << oss) + 2) >> 2; | ||
| + | x1 = ac3 * b6 >> 13; | ||
| + | x2 = (b1 * (b6 * b6 >> 12)) >> 16; | ||
| + | x3 = ((x1 + x2) + 2) >> 2; | ||
| + | b4 = ac4 * (uint32_t) (x3 + 32768) >> 15; | ||
| + | b7 = ((uint32_t) up - b3) * (50000 >> oss); | ||
| + | p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1; | ||
| + | x1 = (p >> 8) * (p >> 8); | ||
| + | x1 = (x1 * 3038) >> 16; | ||
| + | x2 = (-7357 * p) >> 16; | ||
| + | p = p + ((x1 + x2 + 3791) >> 4); | ||
| + | p += BMP_P_ERROR; | ||
| + | return p; | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Measures and returns the airpressure [Pa] | ||
| + | * value. | ||
| + | * The temperature [°C] is stored in the global | ||
| + | * long variable t. | ||
| + | * The pressure and temperature raw values are | ||
| + | * also stored in the global long variables ut | ||
| + | * and up. | ||
| + | * | ||
| + | * Output: Airpressure [Pa] | ||
| + | * | ||
| + | */ | ||
| + | int32_t BMP085_measure(void) | ||
| + | { | ||
| + | BMP085_read(); // Read data | ||
| + | return (BMP085_calculate()); // Result in [Pa] | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Reduces the absolute airpressure [Pa] value | ||
| + | * and returns the airpressure at sea level | ||
| + | * value [Pa]. The airpressure at sea level is | ||
| + | * also stored in the global long variable p0. | ||
| + | * | ||
| + | * Input: p -> Airpressure [Pa] | ||
| + | * alt -> Altitude [m] | ||
| + | * t -> Temperature [/ 10 °C] | ||
| + | * | ||
| + | * Output: Airpressure at sea level [Pa] | ||
| + | * | ||
| + | */ | ||
| + | int32_t BMP085_reducePressure(int32_t p, uint16_t alt, int32_t t) | ||
| + | { | ||
| + | double dp0; | ||
| + | double dt = t / 10; | ||
| + | |||
| + | dp0 = (double) p * (pow ((TEMP_ZERO + dt) | ||
| + | / (TEMP_ZERO + dt + 0.0065 * alt), -5.255)); | ||
| + | p0 = (int32_t) dp0; | ||
| + | return p0; // Result in [Pa] | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * Calculates and returns the altitude [m] by | ||
| + | * using the absolute airpressure [Pa] value. | ||
| + | * | ||
| + | * Input: p -> Airpressure [Pa] | ||
| + | * p0 -> Airpressure at sea level | ||
| + | * [Pa] | ||
| + | * offset -> Altitude offset [m] | ||
| + | * | ||
| + | * Output: Altitude [m] | ||
| + | * | ||
| + | * Hint: If p0 (airpressure at sea level) is | ||
| + | * unknown, set p0 to zero and adjust the | ||
| + | * altitude result with offset! | ||
| + | * | ||
| + | */ | ||
| + | int16_t BMP085_getAltitude(int32_t p, int32_t p0, int16_t offset) | ||
| + | { | ||
| + | float palt; | ||
| + | int16_t alt; | ||
| + | |||
| + | if ((p0 == 0) || (p0 < p)) p0 = 101325; | ||
| + | palt = pow (((double) p / p0), (1.0 / 5.255)); | ||
| + | palt = 1 - palt; | ||
| + | palt = palt / 0.0000225577; | ||
| + | alt = (int16_t) palt; | ||
| + | alt += offset; | ||
| + | return alt; // Result in [m] | ||
| + | } | ||
| + | #endif | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Weather library initialisation: | ||
| + | |||
| + | /** | ||
| + | * Call this once before using the airpressure | ||
| + | * sensor. | ||
| + | * | ||
| + | */ | ||
| + | void weather_init(void) | ||
| + | { | ||
| + | #ifdef HUMIDITY_TEMP_SENSOR | ||
| + | HUMIDITY_TEMP_init(); | ||
| + | #endif | ||
| + | #ifdef AIRPRESSURE_SENSOR | ||
| + | AIRPRESSURE_init(); | ||
| + | #endif | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /****************************************************************************** | ||
| + | * Additional info | ||
| + | * **************************************************************************** | ||
| + | * Changelog: | ||
| + | * - v. 1.0 (initial release) 09.07.2013 by Dirk | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // EOF | ||
| + | </pre> | ||
======Erklärung====== | ======Erklärung====== | ||
| + | |||
=====Demo===== | =====Demo===== | ||
| + | |||
| + | makefile: | ||
| + | <pre>... | ||
| + | TARGET = RP6M256_MultiIO_06 | ||
| + | ... | ||
| + | SRC += RP6M256_WeatherLib.c | ||
| + | ...</pre> | ||
| + | |||
| + | Datei RP6M256_MultiIO_06.c: | ||
| + | <pre>/* | ||
| + | * **************************************************************************** | ||
| + | * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples | ||
| + | * **************************************************************************** | ||
| + | * Example: RP6M256 MultiIO | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Description: | ||
| + | * In this example we show a sixth test for the MultiIO Project Board. | ||
| + | * It tests the "Weather Sensors" (for brightness, humidity, temperature | ||
| + | * and air pressure). | ||
| + | * | ||
| + | * The following sensors are used: | ||
| + | * - Two LDR A9060 brightness sensors (CONRAD No. 145475) | ||
| + | * - HYT-221 humidity & temperature module (CONRAD No. 505671) | ||
| + | * - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085 | ||
| + | * air pressure sensor | ||
| + | * | ||
| + | * ############################################################################ | ||
| + | * The Robot does NOT move in this example! You can simply put it on a table | ||
| + | * next to your PC and you should connect it to the PC via the USB Interface! | ||
| + | * You should also connect to it via WIFI. | ||
| + | * ############################################################################ | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Includes: | ||
| + | |||
| + | #include "RP6M256Lib.h" // The RP6 M256 Library. | ||
| + | // Always needs to be included! | ||
| + | #include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | /*****************************************************************************/ | ||
| + | // Include our new "RP6M256 Weather library": | ||
| + | // (This is the library for accessing certain sensors connected to the | ||
| + | // MultiIO Project Board!) | ||
| + | |||
| + | #include "RP6M256_WeatherLib.h" | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Defines: | ||
| + | |||
| + | //#define SHOW_BMP085_EEPROM_DATA | ||
| + | #define ALT_OFFSET 5 | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | /** | ||
| + | * Write a floating point number to the WIFI. | ||
| + | * | ||
| + | * Example: | ||
| + | * | ||
| + | * // Write a floating point number to the WIFI (no exponent): | ||
| + | * writeDouble_WIFI(1234567.890, 11, 3); | ||
| + | * | ||
| + | * The value of prec (precision) defines the number of decimal places. | ||
| + | * For 32 bit floating point variables (float, double ...) 6 is | ||
| + | * the max. value for prec (7 relevant digits). | ||
| + | * The value of width defines the overall number of characters in the | ||
| + | * floating point number including the decimal point. The number of | ||
| + | * pre-decimal positions is: (width - prec - 1). | ||
| + | */ | ||
| + | void writeDouble_WIFI(double number, uint8_t width, uint8_t prec) | ||
| + | {char buffer[width + 1]; | ||
| + | dtostrf(number, width, prec, &buffer[0]); | ||
| + | writeString_WIFI(&buffer[0]); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Error handler | ||
| + | |||
| + | /** | ||
| + | * This function gets called automatically if there was an I2C Error like | ||
| + | * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30). | ||
| + | * | ||
| + | */ | ||
| + | void I2C_transmissionError(uint8_t errorState) | ||
| + | { | ||
| + | writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x"); | ||
| + | writeInteger_WIFI(errorState, HEX); | ||
| + | writeChar_WIFI('\n'); | ||
| + | } | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Main function - The program starts here: | ||
| + | |||
| + | int main(void) | ||
| + | { | ||
| + | initRP6M256(); // Always call this first! The Processor will not work | ||
| + | // correctly otherwise. | ||
| + | |||
| + | initLCD(); // Initialize the LC-Display (LCD) | ||
| + | // Always call this before using the LCD! | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | mSleep(500); | ||
| + | setLEDs(0b0000); | ||
| + | |||
| + | writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 6!\n"); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation | ||
| + | // with 100kHz SCL Frequency | ||
| + | |||
| + | |||
| + | // Register the event handler: | ||
| + | I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); | ||
| + | |||
| + | setLEDs(0b1111); | ||
| + | |||
| + | // Write a text message to the LCD: | ||
| + | showScreenLCD("################", "################"); | ||
| + | mSleep(1500); | ||
| + | showScreenLCD("RP6v2-M256-WIFI ", "Example Program"); | ||
| + | mSleep(2500); | ||
| + | showScreenLCD("RP6M256 Multi IO", " Selftest 6"); | ||
| + | mSleep(2500); | ||
| + | clearLCD(); | ||
| + | |||
| + | setLEDs(0b0000); | ||
| + | |||
| + | // --------------------------------------- | ||
| + | |||
| + | #ifdef BRIGHTNESS_SENSORS | ||
| + | int16_t lux_l, lux_r; | ||
| + | #endif | ||
| + | #ifdef HUMIDITY_TEMP_SENSOR | ||
| + | double rhum; | ||
| + | #endif | ||
| + | #ifdef AIRPRESSURE_SENSOR | ||
| + | int32_t airpressure; | ||
| + | int32_t airpressure0; | ||
| + | int16_t calcalt; | ||
| + | #endif | ||
| + | |||
| + | startStopwatch1(); | ||
| + | |||
| + | // IMPORTANT: | ||
| + | weather_init(); // Weather init! | ||
| + | |||
| + | while(true) | ||
| + | { | ||
| + | if(getStopwatch1() > 1000) // 1s | ||
| + | { | ||
| + | #ifdef BRIGHTNESS_SENSORS | ||
| + | // Brightness sensors test: | ||
| + | clearLCD(); | ||
| + | lux_l = measureBRIGHTNESS(LEFT); // Read left LDR1 | ||
| + | setCursorPosLCD(0, 0); | ||
| + | writeStringLCD("ADC "); | ||
| + | writeIntegerLCD(adcldr1_l, DEC); | ||
| + | writeStringLCD(" "); | ||
| + | lux_r = measureBRIGHTNESS(RIGHT); // Read right LDR2 | ||
| + | setCursorPosLCD(0, 10); | ||
| + | writeIntegerLCD(adcldr2_r, DEC); | ||
| + | writeStringLCD(" "); | ||
| + | setCursorPosLCD(1, 0); | ||
| + | writeStringLCD("LUX "); | ||
| + | writeIntegerLCD(lux_l, DEC); | ||
| + | writeStringLCD(" "); | ||
| + | setCursorPosLCD(1, 10); | ||
| + | writeIntegerLCD(lux_r, DEC); | ||
| + | writeStringLCD(" "); | ||
| + | #endif | ||
| + | #ifdef HUMIDITY_TEMP_SENSOR | ||
| + | // Humidity & temperature sensor test: | ||
| + | rhum = HYT221_measure(); // Read humidity, temperature | ||
| + | writeString_WIFI("\nHYT-221 SENSOR ->"); | ||
| + | writeString_WIFI("\n- Humidity: "); | ||
| + | writeDouble_WIFI(rhum, 5, 1); | ||
| + | writeString_WIFI(" %\n- Temperature: "); | ||
| + | writeDouble_WIFI(ht, 5, 1); | ||
| + | writeString_WIFI(" °C\n"); | ||
| + | #endif | ||
| + | #ifdef AIRPRESSURE_SENSOR | ||
| + | // Airpressure sensor test: | ||
| + | writeString_WIFI("\nBMP085 SENSOR ->"); | ||
| + | #ifdef SHOW_BMP085_EEPROM_DATA | ||
| + | writeString_WIFI("\n- Calibration data:"); | ||
| + | writeString_WIFI("\n AC1: "); | ||
| + | writeInteger_WIFI(ac1, DEC); | ||
| + | writeString_WIFI("\n AC2: "); | ||
| + | writeInteger_WIFI(ac2, DEC); | ||
| + | writeString_WIFI("\n AC3: "); | ||
| + | writeInteger_WIFI(ac3, DEC); | ||
| + | writeString_WIFI("\n AC4: "); | ||
| + | write32BitInteger_WIFI(ac4, DEC); | ||
| + | writeString_WIFI("\n AC5: "); | ||
| + | write32BitInteger_WIFI(ac5, DEC); | ||
| + | writeString_WIFI("\n AC6: "); | ||
| + | write32BitInteger_WIFI(ac6, DEC); | ||
| + | writeString_WIFI("\n B1: "); | ||
| + | writeInteger_WIFI(b1, DEC); | ||
| + | writeString_WIFI("\n B2: "); | ||
| + | writeInteger_WIFI(b2, DEC); | ||
| + | writeString_WIFI("\n MB: "); | ||
| + | writeInteger_WIFI(mb, DEC); | ||
| + | writeString_WIFI("\n MC: "); | ||
| + | writeInteger_WIFI(mc, DEC); | ||
| + | writeString_WIFI("\n MD: "); | ||
| + | writeInteger_WIFI(md, DEC); | ||
| + | writeChar_WIFI('\n'); | ||
| + | #endif | ||
| + | airpressure = BMP085_measure(); // Read pressure, temperature | ||
| + | writeString_WIFI("\n- Air pressure ["); | ||
| + | writeIntegerLength_WIFI(ALTITUDE, DEC, 4); | ||
| + | writeString_WIFI(" m]: "); | ||
| + | write32BitInteger_WIFI(airpressure, DEC); | ||
| + | // Reduce airpressure: | ||
| + | airpressure0 = BMP085_reducePressure(airpressure, ALTITUDE, t); | ||
| + | writeString_WIFI(" Pa\n- Air pressure [0000 m]: "); | ||
| + | write32BitInteger_WIFI(airpressure0, DEC); | ||
| + | writeString_WIFI(" Pa\n- Temperature ["); | ||
| + | writeIntegerLength_WIFI(ALTITUDE, DEC, 4); | ||
| + | writeString_WIFI(" m]: "); | ||
| + | write32BitInteger_WIFI(t, DEC); | ||
| + | // Calculate altitude: | ||
| + | calcalt = BMP085_getAltitude(airpressure, airpressure0, ALT_OFFSET); | ||
| + | writeString_WIFI(" / 10 °C\n- Calculated altitude : "); | ||
| + | writeIntegerLength_WIFI(calcalt, DEC, 4); | ||
| + | writeString_WIFI(" m\n"); | ||
| + | #endif | ||
| + | setStopwatch1(0); | ||
| + | } | ||
| + | |||
| + | task_I2CTWI(); | ||
| + | } | ||
| + | |||
| + | return 0; | ||
| + | } | ||
| + | </pre> | ||
======Erklärung====== | ======Erklärung====== | ||
| Zeile 3.188: | Zeile 6.994: | ||
Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6M256_DCFLib.c an. | Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6M256_DCFLib.c an. | ||
| − | Die folgenden Zeilen: | + | Die folgenden Zeilen am Anfang der Datei im Abschnitt "Defines": |
<pre>// RP6 M256 WIFI DCF Receiver Connection: | <pre>// RP6 M256 WIFI DCF Receiver Connection: | ||
#define DCF_PORT PORTJ | #define DCF_PORT PORTJ | ||
| Zeile 3.195: | Zeile 7.001: | ||
#define DCF_IN INT1_PI12 // PINJ3 XBUS Pin 8 | #define DCF_IN INT1_PI12 // PINJ3 XBUS Pin 8 | ||
</pre> | </pre> | ||
| − | ... | + | ... müssen so geändert werden: |
<pre>// RP6 M256 WIFI DCF Receiver Connection: | <pre>// RP6 M256 WIFI DCF Receiver Connection: | ||
#define DCF_PORT PORTE | #define DCF_PORT PORTE | ||
| Zeile 3.240: | Zeile 7.046: | ||
'''Zu *)''' TX nicht an Pin 6 des I/O Steckers der M128 vorhanden! | '''Zu *)''' TX nicht an Pin 6 des I/O Steckers der M128 vorhanden! | ||
| − | '''Zu **)''' Auf der M128 ist hier ebenfalls der Piezo-Signalgeber (SND) angeschlossen! | + | '''Zu **)''' Auf der M128 ist hier ebenfalls der Piezo-Signalgeber (SND) angeschlossen! |
'''Stecker ADC-Mxxx:''' | '''Stecker ADC-Mxxx:''' | ||
| Zeile 3.275: | Zeile 7.081: | ||
Wenn jemand die Multi IO Platine unbedingt mit der M128 betreiben will, | Wenn jemand die Multi IO Platine unbedingt mit der M128 betreiben will, | ||
bin ich mit Tips für das Umschreiben der Library auf CompactC gern dabei! | bin ich mit Tips für das Umschreiben der Library auf CompactC gern dabei! | ||
| + | |||
| + | '''Abweichende IO-Jumperstellung für die M128''' | ||
| + | |||
| + | [[Bild:MultiIO_IO-JUMPER_M128.PNG|Jumperstellung für die M128]] | ||
| + | ''Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!'' | ||
| + | |||
| + | '''Zwei Signalgeber am selben Portpin''' | ||
| + | |||
| + | '''WICHTIGER HINWEIS:''' Es ist nur möglich, den Signalgeber (Buzzer SG2) auf der Multi IO Platine zu nutzen, wenn der Signalgeber (SND) auf der M128 deaktiviert wurde (Jumper "EN_SND" auf der M128 abziehen)! Wenn beide Signalgeber GLEICHZEITIG mit dem Portpin PE3 der M128 verbunden sind, kann dieser evtl. beschädigt werden!!! Soll der Buzzer auf der MultiIO NICHT genutzt werden, muss der Jumper "Buz" auf Pin 5 des Jumper-Blocks "J_IO" abgezogen werden. Siehe folgende Abbildung: | ||
| + | |||
| + | [[Bild:MultiIO_J_IO-Mxxx_3.PNG|Buzzer abtrennen]] | ||
| + | ''Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!'' | ||
| + | |||
| + | '''I2C-Adresse des TCN75A''' | ||
| + | |||
| + | '''WICHTIGER HINWEIS:''' Wird die RP6 CCPRO M128 zur Ansteuerung der MultiIO benutzt ODER ist eine RP6 CCPRO M128 gemeinsam mit der MultiIO mit dem XBUS des RP6-Systems verbunden, dann befinden sich ZWEI TCN75A (auf der MultiIO UND der M128) am selben I2C-Bus! Beide Sensoren sind standardmäßig auf dieselbe I2C-Adresse (0x90) eingestellt. | ||
| + | |||
| + | Einer der beiden TCN75A muss deshalb auf eine andere I2C-Adresse umgestellt werden (im Bild oben ist mit "J_TCN_ADR" eine neue '''Standard-I2C-Adresse 0x92''' für den TCN75A auf der MultiIO eingestellt)!!! | ||
| + | |||
| + | Alternativ: Der TCN75A (U26) auf der MultiIO wird deaktiviert ("J_TCN" abgezogen)! Siehe nachfolgende Abbildung: | ||
| + | |||
| + | [[Bild:MultiIO_J_TCN.PNG|TCN75A deaktivieren]] | ||
| + | |||
| + | '''Analoge Sensoren auf dem Bumper Board''' | ||
| + | |||
| + | Mit dem Wahl-Jumper-Block (s.o. Abbildung "Abweichende IO-Jumperstellung für die M128"!) sind die Signalleitungen ("SigL" und "SigR") des Bumper Boards mit "SCL" und "SDA", also mit dem I2C-Bus verbunden (z.B. für SRF02). Will man stattdessen analoge Sensoren (SHARP, Radar) auf dem Bumper Board nutzen (mit der Linienfolger und Bumper Board Library für die M128), dann kann man mit dem Wahl-Jumper-Block leider keine analogen (ADC-) Eingänge dafür auswählen, weil "PF4" und "PF3" nur auf der M256 als ADC-Eingänge verfügbar sind. | ||
| + | |||
| + | Eine mögliche Lösung ist die Nutzung der beiden freien ADC-Kanäle 7 und 2 der M128 für die analogen Sensoren auf dem Bumper Board. Die folgende Abbildung zeigt die dafür nötigen Verbindungen: | ||
| + | |||
| + | [[Bild:MultiIO_IO-JUMPER_AB_M128.PNG|Analoge Bumper Sensoren anschliessen]] | ||
| + | |||
| + | '''Linienfolger Sensoren (LFS) anschliessen''' | ||
| + | |||
| + | Will man die Linienfolger Sensoren (LFS) anstelle der 3,3V-Messung und des Touch Sensors nutzen (mit der Linienfolger und Bumper Board Library für die M128), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block: | ||
| + | |||
| + | [[Bild:MultiIO_Wahl-Jumper_2.PNG|LFS Jumper Stellung]] | ||
| + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | ||
| + | |||
| + | |||
| + | =====Library===== | ||
| + | Datei RP6MultiIOCClib.cc: | ||
| + | '''Die Library ist nicht vollständig!''' | ||
| + | Aktuell sind dies nur die Definitionen, die für die MultiIO benötigt werden! | ||
| + | |||
| + | <pre>/* **************************************************************************** | ||
| + | * _______________________ | ||
| + | * \| RP6 ROBOT SYSTEM |/ | ||
| + | * \_-_-_-_-_-_-_-_-_-_/ >>> CCPRO M128 | ||
| + | * ---------------------------------------------------------------------------- | ||
| + | * ---------------------------- [c]2013 - Dirk -------------------------------- | ||
| + | * **************************************************************************** | ||
| + | * File: RP6MultiIOCClib.cc | ||
| + | * Version: 1.0 - CompactC | ||
| + | * Target: RP6 CCPRO M128 - C-Control PRO M128 @14.7456MHz | ||
| + | * mit optionalem LC-Display 16x2 Zeichen (CONRAD 190911) | ||
| + | * [oder Backlight Display 16x2 Zeichen (CONRAD 191621)] | ||
| + | * und mit der MultiIO Projekt Platine ... | ||
| + | * Author(s): Dirk | ||
| + | * **************************************************************************** | ||
| + | * Beschreibung: | ||
| + | * Dies ist meine RP6 CCPRO M128 MultiIO Bibliothek. Sie enthält Funktionen, | ||
| + | * um die MultiIO Projekt Platine mit der RP6 CCPRO M128 anzusteuern. | ||
| + | * Die Dokumentation zum Projekt ist hier zu finden: | ||
| + | * ---> http://www.rn-wissen.de/index.php/RP6_Multi_IO_Projekt <--- | ||
| + | * ---> http://www.rn-wissen.de/index.php/RP6_Multi_IO_Projekt_-_Software <--- | ||
| + | * | ||
| + | * ... | ||
| + | * | ||
| + | * **************************************************************************** | ||
| + | * CHANGELOG FINDEN SIE AM ENDE DIESER DATEI! | ||
| + | * **************************************************************************** | ||
| + | */ | ||
| + | |||
| + | // Alles nur EINMAL einbinden! | ||
| + | #ifndef __RP6MULTIIOCCLIB__ | ||
| + | #define __RP6MULTIIOCCLIB__ | ||
| + | |||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Auf der MultiIO montierte Komponenten: | ||
| + | // - I2C Spannung & Strom Sensor (LTC2990) | ||
| + | // - I2C Echtzeituhr (RTC DS1307Z) | ||
| + | // - I2C Temperatur Sensor (TCN75A) | ||
| + | // - I2C Servo Controller (PCA9685) | ||
| + | // - I2C EEPROM (24LCXXX) | ||
| + | // - 3V3 Spannung Sensor | ||
| + | // - Touch Sensor (mit NE555) | ||
| + | // - Buttons | ||
| + | // - LEDs | ||
| + | // - Buzzer | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Konfiguration: | ||
| + | |||
| + | // I2C Spannung & Strom Sensor (LTC2990): | ||
| + | #define I2C_MULTIIO_VCS_ADR 0x98 // ADR1/0 = 0/0 | ||
| + | //#define I2C_MULTIIO_VCS_ADR 0x9a // ADR1/0 = 0/1 | ||
| + | //#define I2C_MULTIIO_VCS_ADR 0x9c // ADR1/0 = 1/0 | ||
| + | //#define I2C_MULTIIO_VCS_ADR 0x9e // ADR1/0 = 1/1 | ||
| + | //#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address | ||
| + | |||
| + | // (Abgleich von Shunt Widerstand, Strom und Spannung!) | ||
| + | #define SHUNT_R 0.051 // R10 [Ohm] | ||
| + | #define C_ADJUST 2.0 // Strom Abgleich-Faktor | ||
| + | #define C_OFFSET 0.0 // Strom Offset [mA] | ||
| + | #define VBAT_ADJUST 3.2 // (R13 + R14) / R14 | ||
| + | #define VSERVO_ADJUST 2.5 // (R32 + R22) / R22 | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Echtzeituhr (RTC DS1307Z): | ||
| + | #define I2C_MULTIIO_RTC_ADR 0xD0 // Default | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Temperatur Sensor (TCN75A): | ||
| + | // (A1 immer 0!) | ||
| + | //#define I2C_MULTIIO_TEMP_ADR 0x90 // A2/0 = 0/0 | ||
| + | #define I2C_MULTIIO_TEMP_ADR 0x92 // A2/0 = 0/1 | ||
| + | //#define I2C_MULTIIO_TEMP_ADR 0x98 // A2/0 = 1/0 | ||
| + | //#define I2C_MULTIIO_TEMP_ADR 0x9a // A2/0 = 1/1 | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C Servo Controller (PCA9685): | ||
| + | // (A5, A4, A3, A2 immer 0!) | ||
| + | #define I2C_MULTIIO_SERVO_ADR 0x80 // A1/0 = 0/0 | ||
| + | //#define I2C_MULTIIO_SERVO_ADR 0x82 // A1/0 = 0/1 | ||
| + | //#define I2C_MULTIIO_SERVO_ADR 0x84 // A1/0 = 1/0 | ||
| + | //#define I2C_MULTIIO_SERVO_ADR 0x86 // A1/0 = 1/1 | ||
| + | //#define I2C_MULTIIO_SERVO_ADR 0xe0 // ALLCALLADR | ||
| + | |||
| + | // (Servo Power ist verbunden mit LED8 des PCA9685!) | ||
| + | #define CHSERVOPWR 9 | ||
| + | |||
| + | // Konstanten für linker Servo Anschlag (LT), rechter Servo Anschlag (RT), | ||
| + | // mittlere Servo Position (MP): | ||
| + | // (Hinweise: - Servo Impuls Länge [ms] = Servo Positionswert / 204.8 | ||
| + | // (Formel nur gültig für eine PWM von 50 Hz!) | ||
| + | // - Min. Servo Impuls (0,7 ms) = Servo Positionswert 143 | ||
| + | // - Mid. Servo Impuls (1,5 ms) = Servo Positionswert 307 | ||
| + | // - Max. Servo Impuls (2,3 ms) = Servo Positionswert 471 | ||
| + | // - !!! Keine Positionswerte < 143 oder > 471 benutzen!!!) | ||
| + | #define SERVO1_LT 205 // Servo Impuls ~1ms | ||
| + | #define SERVO1_RT 410 // Servo Impuls ~2ms | ||
| + | #define SERVO1_MP ((SERVO1_RT - SERVO1_LT) / 2 + SERVO1_LT) | ||
| + | #define SERVO2_LT 205 | ||
| + | #define SERVO2_RT 410 | ||
| + | #define SERVO2_MP ((SERVO2_RT - SERVO2_LT) / 2 + SERVO2_LT) | ||
| + | #define SERVO3_LT 205 | ||
| + | #define SERVO3_RT 410 | ||
| + | #define SERVO3_MP ((SERVO3_RT - SERVO3_LT) / 2 + SERVO3_LT) | ||
| + | #define SERVO4_LT 205 | ||
| + | #define SERVO4_RT 410 | ||
| + | #define SERVO4_MP ((SERVO4_RT - SERVO4_LT) / 2 + SERVO4_LT) | ||
| + | #define SERVO5_LT 205 | ||
| + | #define SERVO5_RT 410 | ||
| + | #define SERVO5_MP ((SERVO5_RT - SERVO5_LT) / 2 + SERVO5_LT) | ||
| + | #define SERVO6_LT 205 | ||
| + | #define SERVO6_RT 410 | ||
| + | #define SERVO6_MP ((SERVO6_RT - SERVO6_LT) / 2 + SERVO6_LT) | ||
| + | #define SERVO7_LT 205 | ||
| + | #define SERVO7_RT 410 | ||
| + | #define SERVO7_MP ((SERVO7_RT - SERVO7_LT) / 2 + SERVO7_LT) | ||
| + | #define SERVO8_LT 205 | ||
| + | #define SERVO8_RT 410 | ||
| + | #define SERVO8_MP ((SERVO8_RT - SERVO8_LT) / 2 + SERVO8_LT) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // I2C EEPROM (24LCXXX): | ||
| + | // (A2=1 nicht mit 24LC1024-P nutzbar!) | ||
| + | #define I2C_MULTIIO_EEPROM_ADR 0xA0 // A2/1/0 = 0/0/0 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xA2 // A2/1/0 = 0/0/1 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xA4 // A2/1/0 = 0/1/0 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xA6 // A2/1/0 = 0/1/1 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xA8 // A2/1/0 = 1/0/0 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xAA // A2/1/0 = 1/0/1 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xAC // A2/1/0 = 1/1/0 | ||
| + | //#define I2C_MULTIIO_EEPROM_ADR 0xAE // A2/1/0 = 1/1/1 | ||
| + | |||
| + | // Definiere die I2C-EEPROM Speicherkapazität [kbit] hier: | ||
| + | #define I2C_EEPROM_KBIT 32 // 24LC32-P <== Standard | ||
| + | //#define I2C_EEPROM_KBIT 64 // 24LC64-P | ||
| + | //#define I2C_EEPROM_KBIT 128 // 24LC128-P | ||
| + | //#define I2C_EEPROM_KBIT 256 // 24LC256-P | ||
| + | //#define I2C_EEPROM_KBIT 512 // 24LC512-P | ||
| + | //#define I2C_EEPROM_KBIT 1024 // 24LC1024-P | ||
| + | |||
| + | // Definiere die I2C-EEPROM Seitengröße [byte] hier: | ||
| + | // ACHTUNG: Die Seitengröße muss zum oben definierten EEPROM Typ passen! | ||
| + | #define I2C_EEPROM_PAGESIZE 32 // EEPROM 32 or 64 kbit | ||
| + | //#define I2C_EEPROM_PAGESIZE 64 // EEPROM 128 or 256 kbit | ||
| + | //#define I2C_EEPROM_PAGESIZE 128 // EEPROM 512 kbit | ||
| + | //#define I2C_EEPROM_PAGESIZE 256 // EEPROM 1024 kbit | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // 3V3 Spannung Sensor: | ||
| + | #define ADC_MULTIIO_3V3 3 // PortF.3 ADC-Mxxx: ADC | ||
| + | //#define ADC_MULTIIO_3V3 7 // PortF.7 ADC-Mxxx: ADC | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Touch Sensor (mit NE555): | ||
| + | #define ADC_MULTIIO_TOUCH 1 // PortF.1 ADC-Mxxx: ADC | ||
| + | |||
| + | #define ADCVAL_NOTOUCH 15 | ||
| + | #define ADCVAL_TOUCH 1020 | ||
| + | #define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Buttons: | ||
| + | #define ADC_MULTIIO_BUTTONS 6 // PortF.6 ADC-Mxxx: ADC | ||
| + | |||
| + | #define ADCVAL_BUTTON1 14 | ||
| + | #define ADCVAL_BUTTON2 577 | ||
| + | #define ADCVAL_BUTTON3 736 | ||
| + | #define ADCVAL_BUTTON4 812 | ||
| + | #define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) | ||
| + | #define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) | ||
| + | #define ADCVAL_LIMIT34 ((ADCVAL_BUTTON4 - ADCVAL_BUTTON3) / 2 + ADCVAL_BUTTON3) | ||
| + | #define ADCVAL_LIMIT40 ((1023 - ADCVAL_BUTTON4) / 2 + ADCVAL_BUTTON4) | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // LEDs: | ||
| + | // (Status LED1..LED4 sind verbunden mit LED15..LED12 des PCA9685!) | ||
| + | #define CHLED1 16 | ||
| + | #define CHLED2 15 | ||
| + | #define CHLED3 14 | ||
| + | #define CHLED4 13 | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Buzzer: | ||
| + | // (Der IO-Mxxx Stecker ist verbunden mit dem M128 I/O Stecker!) | ||
| + | #define IO_MULTIIO_BUZZER_PORT 35 // PortE.3 IO-Mxxx: I/O | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | // Andere ADC Kanal Definitionen: | ||
| + | // (Abhängig von Jumper-Stellungen auf der MultiIO Projekt Platine!) | ||
| + | // (Der ADC-Mxxx Stecker ist verbunden mit dem M128 ADC Stecker!) | ||
| + | #define ADC_MULTIIO_LFS_L 1 // PortF.1 ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_LFS_M 3 // PortF.3 ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_LFS_R 5 // PortF.5 ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_BUMPER_L 0 // PortF.0 ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_BUMPER_R 4 // PortF.4 ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_SHARP_L 7 // PortF.7 ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_SHARP_R 2 // PortF.2 ADC-Mxxx: ADC | ||
| + | |||
| + | // Andere IO Portpin Definitionen: | ||
| + | // (Der IO-Mxxx Stecker ist verbunden mit dem M128 I/O Stecker!) | ||
| + | #define IO_MULTIIO_LFS_PWR_PORT 31 // PortD.7 IO-Mxxx: I/O | ||
| + | #define IO_MULTIIO_SHARPS_PWR_PORT 14 // PortB.6 IO-Mxxx: I/O | ||
| + | #define IO_MULTIIO_SNAKE_SWITCH_PORT 30 // PortD.6 IO-Mxxx: I/O | ||
| + | #define IO_MULTIIO_SNAKE_KEY_PORT 27 // PortD.3 IO-Mxxx: I/O | ||
| + | #define IO_MULTIIO_DCF77_PORT 13 // PortB.5 IO-Mxxx: I/O | ||
| + | |||
| + | /*****************************************************************************/ | ||
| + | |||
| + | // Ende Definitionen | ||
| + | </pre> | ||
| + | |||
| + | ======Erklärung====== | ||
=====Demo===== | =====Demo===== | ||
| + | |||
| + | ======Erklärung====== | ||
| + | |||
| + | ====DCF77-Library==== | ||
| + | Die "Funkuhr-Zeit" kann empfangen und ausgewertet werden, wenn ein DCF77-Empfänger (z.B. CONRAD 641138) mit dem dafür vorgesehenen Anschluß "DCF77" auf der Multi IO Platine verbunden ist. | ||
| + | |||
| + | '''Die RP6 CCPRO M128 DCF77 Library in CompactC findet ihr [http://www.roboternetz.de/community/threads/42859-CCPRO-M128-DCF77-Decoder-1-(CompactC) HIER] (Teil 1) und [http://www.roboternetz.de/community/threads/42860-CCPRO-M128-DCF77-Decoder-2-(CompactC) HIER] (Teil 2)!''' | ||
| + | |||
| + | Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6DCFCClib.cc an. | ||
| + | |||
| + | Die folgenden Zeilen am Anfang der Datei im Abschnitt "Defines": | ||
| + | <pre>// RP6 CCPRO M128 DCF Empfänger Anschluß: | ||
| + | #define DCF_IN 37 // DCF Eingangs Signale an PortE.5 (RP6: INT1) | ||
| + | </pre> | ||
| + | ... müssen so geändert werden: | ||
| + | <pre>// RP6 CCPRO M128 DCF Empfänger Anschluß: | ||
| + | #define DCF_IN 13 // DCF Eingangs Signale an PortB.5 (M128: OC1A) | ||
| + | </pre> | ||
| + | |||
| + | ====Weitere Software==== | ||
| + | |||
| + | Weitere Software einschließlich Libraries für den Betrieb | ||
| + | der MultiIO Platine mit der RP6 CCPRO M128 findet ihr [https://www.dropbox.com/sh/7fab3gyweezdrcy/l4uw9djFxP hier] | ||
| + | im Verzeichnis "M128" der Datei ''Software.zip''. | ||
===RP6 CONTROL M32=== | ===RP6 CONTROL M32=== | ||
| Zeile 3.358: | Zeile 7.445: | ||
Zu (2): Siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]! | Zu (2): Siehe [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library|hier]]! | ||
| − | Die Library (Software-Bibliothek) besteht aus drei Teilen: | + | '''Abweichende IO-Jumperstellung für die M32''' |
| + | |||
| + | [[Bild:MultiIO_IO-JUMPER_M32.PNG|Jumperstellung für die M32]] | ||
| + | ''Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!'' | ||
| + | |||
| + | '''Analoge Sensoren auf dem Bumper Board''' | ||
| + | |||
| + | Mit dem Wahl-Jumper-Block (s.o. Abbildung "Abweichende IO-Jumperstellung für die M32"!) sind die Signalleitungen ("SigL" und "SigR") des Bumper Boards mit "SCL" und "SDA", also mit dem I2C-Bus verbunden (z.B. für SRF02). Will man stattdessen analoge Sensoren (SHARP, Radar) auf dem Bumper Board nutzen (mit der Linienfolger und Bumper Board Library für die M32), dann kann man mit dem Wahl-Jumper-Block leider keine analogen (ADC-) Eingänge dafür auswählen, weil "PF4" und "PF3" nur auf der M256 als ADC-Eingänge verfügbar sind. | ||
| + | |||
| + | Eine mögliche Lösung ist die Nutzung der beiden freien ADC-Kanäle 2 (ADC_2) und 7 (ADC_7) der M32 für die analogen Sensoren auf dem Bumper Board. Die folgende Abbildung zeigt die dafür nötigen Verbindungen: | ||
| + | |||
| + | [[Bild:MultiIO_IO-JUMPER_AB_M32.PNG|Analoge Bumper Sensoren anschliessen]] | ||
| + | |||
| + | '''Bumper anschliessen''' | ||
| + | |||
| + | Mit der M32 lassen sich leider auch die Bumper Taster auf dem Bumper Board nicht direkt auswerten (mit der Linienfolger und Bumper Board Library für die M32), weil die Signalleitungen ("ON-L" und "ON-R") der Bumper Taster an "J_ADC" Pins 4 und 8 nicht mit Portpins der M32 zu verbinden sind. | ||
| + | |||
| + | Eine mögliche Lösung ist die Nutzung der beiden freien I/O-Portpins PD6 (IO_PD6) und PC4 (IO_PC4) der M32 für die Bumper Taster auf dem Bumper Board. Die folgende Abbildung zeigt die dafür nötigen Verbindungen: | ||
| + | |||
| + | [[Bild:MultiIO_IO-JUMPER_BB_M32.PNG|Bumper Taster anschliessen]] | ||
| + | |||
| + | '''Linienfolger Sensoren (LFS) anschliessen''' | ||
| + | |||
| + | Will man die Linienfolger Sensoren (LFS) anstelle der 3,3V-Messung und des Touch Sensors nutzen (mit der Linienfolger und Bumper Board Library für die M32), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block: | ||
| + | |||
| + | [[Bild:MultiIO_Wahl-Jumper_2.PNG|LFS Jumper Stellung]] | ||
| + | ''Hinweis: Die Jumper sind orange eingezeichnet!'' | ||
| + | |||
| + | |||
| + | Die '''Library''' (Software-Bibliothek) besteht aus drei Teilen: | ||
* Dem [[RP6_Multi_IO_Projekt_-_Software#Configuration Header_2|Configuration Header]] -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | * Dem [[RP6_Multi_IO_Projekt_-_Software#Configuration Header_2|Configuration Header]] -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss. | ||
| − | * Dem [[RP6_Multi_IO_Projekt_-_Software#Library | + | * Dem [[RP6_Multi_IO_Projekt_-_Software#Library Header_6|Library Header]] -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library. |
| − | * Der [[RP6_Multi_IO_Projekt_-_Software#Library | + | * Der [[RP6_Multi_IO_Projekt_-_Software#Library Source_6|Library Source]] -> Das ist die eigentliche Library. |
=====Configuration Header===== | =====Configuration Header===== | ||
| Zeile 3.405: | Zeile 7.521: | ||
//#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address | //#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address | ||
| − | // ( | + | // (Shunt resistor, current and voltage adjustment!) |
| − | #define SHUNT_R 0.051 // | + | #define SHUNT_R 0.051 // R10 [Ohm] |
| − | #define | + | #define C_ADJUST 2.0 // Current adjust factor |
| − | #define | + | #define C_OFFSET 0.0 // Current offset [mA] |
| − | #define VBAT_ADJUST 3.2 // ( | + | #define VBAT_ADJUST 3.2 // (R13 + R14) / R14 |
| − | #define VSERVO_ADJUST 2.5 // ( | + | #define VSERVO_ADJUST 2.5 // (R32 + R22) / R22 |
/*****************************************************************************/ | /*****************************************************************************/ | ||
| Zeile 3.505: | Zeile 7.621: | ||
#define ADC_MULTIIO_TOUCH ADC_3 // ADC-Mxxx: ADC | #define ADC_MULTIIO_TOUCH ADC_3 // ADC-Mxxx: ADC | ||
| − | #define ADCVAL_NOTOUCH | + | #define ADCVAL_NOTOUCH 15 |
| − | #define ADCVAL_TOUCH | + | #define ADCVAL_TOUCH 1020 |
#define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) | #define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) | ||
| Zeile 3.513: | Zeile 7.629: | ||
#define ADC_MULTIIO_BUTTONS ADC_6 // ADC-Mxxx: ADC | #define ADC_MULTIIO_BUTTONS ADC_6 // ADC-Mxxx: ADC | ||
| − | #define ADCVAL_BUTTON1 | + | #define ADCVAL_BUTTON1 14 |
| − | #define ADCVAL_BUTTON2 | + | #define ADCVAL_BUTTON2 577 |
| − | #define ADCVAL_BUTTON3 | + | #define ADCVAL_BUTTON3 736 |
| − | #define ADCVAL_BUTTON4 | + | #define ADCVAL_BUTTON4 812 |
#define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) | #define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) | ||
#define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) | #define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) | ||
| Zeile 3.544: | Zeile 7.660: | ||
#define ADC_MULTIIO_LFS_M ADC_4 // ADC-Mxxx: ADC | #define ADC_MULTIIO_LFS_M ADC_4 // ADC-Mxxx: ADC | ||
#define ADC_MULTIIO_LFS_R ADC_5 // ADC-Mxxx: ADC | #define ADC_MULTIIO_LFS_R ADC_5 // ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_SHARP_L ADC_2 // ADC-Mxxx: ADC | ||
| + | #define ADC_MULTIIO_SHARP_R ADC_7 // ADC-Mxxx: ADC | ||
// Other IO portpin definitions: | // Other IO portpin definitions: | ||
| Zeile 3.566: | Zeile 7.684: | ||
#define IO_MULTIIO_DCF77_DDR DDRC | #define IO_MULTIIO_DCF77_DDR DDRC | ||
#define IO_MULTIIO_DCF77_PIN PINC | #define IO_MULTIIO_DCF77_PIN PINC | ||
| + | |||
| + | #define IO_MULTIIO_BUMPER_R_IN IO_PC4 // IO-Mxxx: I/O | ||
| + | #define IO_MULTIIO_BUMPER_R_DDR DDRC | ||
| + | #define IO_MULTIIO_BUMPER_R_PIN PINC | ||
| + | #define IO_MULTIIO_BUMPER_R_PORT PORTC | ||
| + | |||
| + | #define IO_MULTIIO_BUMPER_L_IN IO_PD6 // IO-Mxxx: I/O | ||
| + | #define IO_MULTIIO_BUMPER_L_DDR DDRD | ||
| + | #define IO_MULTIIO_BUMPER_L_PIN PIND | ||
| + | #define IO_MULTIIO_BUMPER_L_PORT PORTD | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| Zeile 3.934: | Zeile 8.062: | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // MultiIO Project Board initialisation: | + | // MultiIO Project Board initialisation and shutdown: |
void multiio_init(void); | void multiio_init(void); | ||
| + | void multiio_shutdown(void); | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| Zeile 4.088: | Zeile 8.217: | ||
void LTC2990_calculate(void) | void LTC2990_calculate(void) | ||
{ | { | ||
| − | int16_t tmp | + | int16_t tmp; |
| − | double | + | double v2; |
tmp = (((vcs_tint_high & 0x1f) << 8) + vcs_tint_low) << 3; | tmp = (((vcs_tint_high & 0x1f) << 8) + vcs_tint_low) << 3; | ||
| Zeile 4.095: | Zeile 8.224: | ||
tint = tmp / 16.0; // Internal temperature [°C] | tint = tmp / 16.0; // Internal temperature [°C] | ||
| − | tmp = ((( | + | tmp = (((vcs_v2_high & 0x7f) << 8) + vcs_v2_low) << 1; |
tmp = tmp / 2; | tmp = tmp / 2; | ||
| − | |||
| − | |||
| − | |||
| − | |||
v2 = tmp * 0.01942; | v2 = tmp * 0.01942; | ||
| − | v2 *= | + | v2 *= C_ADJUST; // Current adjust factor |
| − | cbat = | + | cbat = v2 / SHUNT_R + C_OFFSET; // Battery current [mA] |
tmp = ((vcs_v3_high & 0x3f) << 8) + vcs_v3_low; | tmp = ((vcs_v3_high & 0x3f) << 8) + vcs_v3_low; | ||
| Zeile 4.834: | Zeile 8.959: | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
/*****************************************************************************/ | /*****************************************************************************/ | ||
| − | // MultiIO Project Board initialisation: | + | // MultiIO Project Board initialisation and shutdown: |
/** | /** | ||
| Zeile 4.851: | Zeile 8.976: | ||
// Buzzer: | // Buzzer: | ||
Buzzer_init(); // Init buzzer | Buzzer_init(); // Init buzzer | ||
| + | } | ||
| + | |||
| + | /** | ||
| + | * If you don't use any function of the MultiIO | ||
| + | * Project Board, you can put the MultiIO into | ||
| + | * "SHUTDOWN MODE". In this mode the electric | ||
| + | * power consumption is very low to save energy. | ||
| + | * | ||
| + | */ | ||
| + | void multiio_shutdown(void) | ||
| + | { | ||
| + | // LEDs: | ||
| + | setMultiIOLEDs(0b0000); // MultiIO LEDs off | ||
| + | // Servo power: | ||
| + | setServoPower(0); // Servo power off | ||
| + | // Servo Controller: | ||
| + | PCA9685_shutdown(); // Shutdown PCA9685 | ||
| + | // Temperature Sensor: | ||
| + | TCN75_shutdown(); // Shutdown TCN75A | ||
} | } | ||
| Zeile 4.860: | Zeile 9.004: | ||
* **************************************************************************** | * **************************************************************************** | ||
* Changelog: | * Changelog: | ||
| − | * - v. 1.0 (initial release) | + | * - v. 1.2 (shutdown mode added) 28.03.2013 by Dirk |
| + | * - v. 1.1 (LTC2990 current bug fixed) 26.03.2013 by Dirk | ||
| + | * - v. 1.0 (initial release) 18.03.2013 by Dirk | ||
* | * | ||
* **************************************************************************** | * **************************************************************************** | ||
| Zeile 4.875: | Zeile 9.021: | ||
makefile: | makefile: | ||
<pre>... | <pre>... | ||
| − | TARGET = | + | TARGET = RP6Control_MultiIO_01 |
... | ... | ||
SRC += RP6Control_MultiIOLib.c | SRC += RP6Control_MultiIOLib.c | ||
...</pre> | ...</pre> | ||
| − | Datei | + | Datei RP6Control_MultiIO_01.c: |
<pre>/* | <pre>/* | ||
| Zeile 5.010: | Zeile 9.156: | ||
rtc_time.hour = 12; // 12:00 | rtc_time.hour = 12; // 12:00 | ||
rtc_date.weekday = R_TH; | rtc_date.weekday = R_TH; | ||
| − | rtc_date.day = | + | rtc_date.day = 28; |
| − | rtc_date.month = | + | rtc_date.month = 3; |
| − | rtc_date.year = 2013; // Do, | + | rtc_date.year = 2013; // Do, 28.3.2013 |
DS1307_write(); | DS1307_write(); | ||
// Remove this, if RTC is set and running!!! | // Remove this, if RTC is set and running!!! | ||
| Zeile 5.032: | Zeile 9.178: | ||
// Buzzer test: | // Buzzer test: | ||
| − | + | buzzer(330); | |
mSleep(200); | mSleep(200); | ||
| − | + | buzzer(330); | |
mSleep(200); | mSleep(200); | ||
| − | + | buzzer(330); | |
mSleep(1000); | mSleep(1000); | ||
| − | + | buzzer(330); | |
mSleep(200); | mSleep(200); | ||
| − | + | buzzer(330); | |
mSleep(200); | mSleep(200); | ||
| − | + | buzzer(330); | |
while(true) | while(true) | ||
| Zeile 5.126: | Zeile 9.272: | ||
writeDouble(vcc, 4, 1); | writeDouble(vcc, 4, 1); | ||
writeString("V\n"); | writeString("V\n"); | ||
| + | |||
| + | // MultiIO shutdown: | ||
| + | if (pressedMultiIOButtonNumber == 4) { | ||
| + | writeString("\nPress button 1 for MultiIO SHUTDOWN"); | ||
| + | writeString("\nor any other button to continue!!!\n"); | ||
| + | do { | ||
| + | mSleep(1); | ||
| + | releasedMultiIOButtonNumber = checkMultiIOReleasedButtonEvent(); | ||
| + | task_I2CTWI(); | ||
| + | } while (!releasedMultiIOButtonNumber); | ||
| + | if (releasedMultiIOButtonNumber == 1) { | ||
| + | writeString("\nPlease wait for MultiIO SHUTDOWN...\n"); | ||
| + | multiio_shutdown(); | ||
| + | mSleep(3000); | ||
| + | writeString("\nThe MultiIO now is in SHUTDOWN MODE!!!\n"); | ||
| + | mSleep(1000); | ||
| + | writeString("\nRESET the M32 microcontroller now...\n\n"); | ||
| + | while(true) {}; | ||
| + | } | ||
| + | } | ||
setStopwatch1(0); | setStopwatch1(0); | ||
| Zeile 5.138: | Zeile 9.304: | ||
======Erklärung====== | ======Erklärung====== | ||
| + | |||
| + | ====DCF77-Library==== | ||
| + | Die "Funkuhr-Zeit" kann empfangen und ausgewertet werden, wenn ein DCF77-Empfänger (z.B. CONRAD 641138) mit dem dafür vorgesehenen Anschluß "DCF77" auf der Multi IO Platine verbunden ist. | ||
| + | |||
| + | '''Die (RP6 BASE und) RP6 CONTROL M32 DCF77 Library findet ihr [http://www.roboternetz.de/community/threads/30731-RP6(Control)-DCF77-Library HIER]!''' | ||
| + | |||
| + | Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6ControlDCFLib.c an. | ||
| + | |||
| + | Die folgenden Zeilen am Anfang der Datei im Abschnitt "Defines": | ||
| + | <pre>// RP6 Control DCF Receiver Connection: | ||
| + | #define DCF_PORT PORTD | ||
| + | #define DCF_DDR DDRD | ||
| + | #define DCF_PIN PIND | ||
| + | #define DCF_IN EINT1 // PIND2 XBUS Pin 8 | ||
| + | </pre> | ||
| + | ... müssen so geändert werden: | ||
| + | <pre>// RP6 Control DCF Receiver Connection: | ||
| + | #define DCF_PORT PORTC | ||
| + | #define DCF_DDR DDRC | ||
| + | #define DCF_PIN PINC | ||
| + | #define DCF_IN IO_PC7 // PINC7 I/O Pin 1 | ||
| + | </pre> | ||
====Servo-Library==== | ====Servo-Library==== | ||
| − | Auf der MultiIO | + | Auf der MultiIO ist ein 10-poliger Wannenstecker "ServosM32" und ein 8x3-poliger Servo-Anschluß-Stecker vorgesehen, um 8 Servos direkt an die RP6Control M32 anschließen zu können. Dazu muss der I/O-Wannenstecker der M32 mit dem o.g. Wannenstecker für die Servos verbunden werden. Man kann dann bis zu 8 Servos an den Servo-Anschluß-Stecker der MultiIO anstecken. |
Dieser M32 Servo-Anschluß auf der MultiIO dient also NUR dazu, das Anschließen von Servos zu erleichtern. Die MultiIO steuert diese Servos NICHT! Sie stellt allerdings die Stromversorgung für die Servos zur Verfügung. Das bedeutet, dass die Servo-Spannung auf der MultiIO eingeschaltet werden muss, wenn man diese Steckverbindung nutzen will. | Dieser M32 Servo-Anschluß auf der MultiIO dient also NUR dazu, das Anschließen von Servos zu erleichtern. Die MultiIO steuert diese Servos NICHT! Sie stellt allerdings die Stromversorgung für die Servos zur Verfügung. Das bedeutet, dass die Servo-Spannung auf der MultiIO eingeschaltet werden muss, wenn man diese Steckverbindung nutzen will. | ||
Wenn man die RP6Control M32 Library für 8 Servos nicht verändern will, | Wenn man die RP6Control M32 Library für 8 Servos nicht verändern will, | ||
| − | muss | + | muss man den Jumper "J_Servos-On" auf der MultiIO aufstecken, siehe |
folgende Abbildung! | folgende Abbildung! | ||
| Zeile 5.151: | Zeile 9.339: | ||
'''Die RP6Control M32 Library für 8 Servos findet ihr [http://www.roboternetz.de/community/showthread.php?40090-RP6Control-M32-Library-für-8-Servos HIER]!''' | '''Die RP6Control M32 Library für 8 Servos findet ihr [http://www.roboternetz.de/community/showthread.php?40090-RP6Control-M32-Library-für-8-Servos HIER]!''' | ||
| + | |||
| + | Wo die 8 Servos (SERVO1..SERVO8) anzuschließen sind zeigt diese Abbildung: | ||
| + | |||
| + | [[Bild:MultiIO_ServosM32.PNG|Servo-Anschluß-Stecker]] | ||
| + | |||
| + | ====Weitere Software==== | ||
| + | Weitere Software einschließlich Libraries für den Betrieb | ||
| + | der MultiIO Platine mit der RP6 CONTROL M32 findet ihr [https://www.dropbox.com/sh/7fab3gyweezdrcy/l4uw9djFxP hier] | ||
| + | im Verzeichnis "M32" der Datei ''Software.zip''. | ||
===RP6 BASE=== | ===RP6 BASE=== | ||
| Zeile 5.157: | Zeile 9.354: | ||
Es gibt auf dem RP6 auch keine passenden Wannenstecker (wie auf der M32, M128 und M256), um auf einfache Weise eine mehrpolige Verbindung zu den IO-Mxxx, ADC-Mxxx oder ADC-M256 Steckern der Multi IO Platine herzustellen. | Es gibt auf dem RP6 auch keine passenden Wannenstecker (wie auf der M32, M128 und M256), um auf einfache Weise eine mehrpolige Verbindung zu den IO-Mxxx, ADC-Mxxx oder ADC-M256 Steckern der Multi IO Platine herzustellen. | ||
| − | Stattdessen kann man einzelne Portpins des RP6 mit der Multi IO Platine verbinden. | + | Stattdessen kann man einzelne Portpins des RP6 mit der Multi IO Platine verbinden. ''Diese Verbindung kann auch über den USRBUS erfolgen.'' |
Die folgende Tabelle zeigt mögliche Verbindungen: | Die folgende Tabelle zeigt mögliche Verbindungen: | ||
| Zeile 5.178: | Zeile 9.375: | ||
| PC5 || TDI || IO2 || I || Status LED2! Zusätzlich als Input z.B. für Taster nutzbar * || DCF77 ... | | PC5 || TDI || IO2 || I || Status LED2! Zusätzlich als Input z.B. für Taster nutzbar * || DCF77 ... | ||
|- | |- | ||
| − | | PD0 || RXD || PRG/U:2 || I/O || UART RX! Nutzbar NUR, wenn USB-Interface abgezogen, als UART RX oder freier IO || | + | | PD0 || RXD || PRG/U:2 || I/O || UART RX! Nutzbar NUR, wenn USB-Interface abgezogen, als UART RX oder freier IO || GPS-Tx, SHARPS_PWR ... |
|- | |- | ||
| − | | PD1 || TXD || PRG/U:3 || I/O || UART TX! Nutzbar NUR, wenn USB-Interface abgezogen, als UART TX oder freier IO || | + | | PD1 || TXD || PRG/U:3 || I/O || UART TX! Nutzbar NUR, wenn USB-Interface abgezogen, als UART TX oder freier IO || GPS-Rx, LFS_PWR ... |
|} | |} | ||
| Zeile 5.193: | Zeile 9.390: | ||
Aus den genannten Gründen kann es keine generelle Multi IO | Aus den genannten Gründen kann es keine generelle Multi IO | ||
Projekt Library für die RP6 BASE geben! Ich bin gern dabei, | Projekt Library für die RP6 BASE geben! Ich bin gern dabei, | ||
| − | wenn jemand die [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library_3|M32 Library]] für die RP6 | + | wenn jemand die [[RP6_Multi_IO_Projekt_-_Software#Multi_IO_Projekt_Library_3|M32 Library]] für die RP6 BASE umschreiben |
möchte! | möchte! | ||
| + | |||
| + | Auf jeden Fall lassen sich alle Teile der M32 Library für | ||
| + | die RP6 BASE übernehmen, die die I2C-Sensoren und -Aktoren | ||
| + | der MultiIO auslesen/ansteuern. Die können problemlos mit | ||
| + | der RP6 BASE genutzt werden, wenn die BASE als I2C-Master | ||
| + | konfiguriert wird. | ||
| + | |||
| + | Folgende Sensoren/Aktoren lassen sich über I2C mit der RP6 | ||
| + | BASE auslesen/ansteuern: | ||
| + | - Spannungs- und Stromsensor (LTC2990) | ||
| + | - Echtzeituhr (RTC DS1307Z) | ||
| + | - Temperatursensor (TCN75A) | ||
| + | - Servo Controller (PCA9685) | ||
| + | - EEPROM (24LCXXX) | ||
| + | - Ultraschalldistanzsensoren (SRF02) | ||
| + | - Luftdrucksensor (BMP085) | ||
| + | - Feuchte-, Temperatursensor (HYT-221) | ||
| + | - 2D-Kompass (HDMM01) | ||
| + | - 9D-IMU (MinIMU-9 v2) | ||
| + | - AM-/FM-Radio mit RDS (Si4735) | ||
| + | |||
| + | Folgender Sensor läßt sich über den UART der RP6 BASE aus- | ||
| + | lesen: | ||
| + | - GPS-Empfänger (NL-552ETTL) | ||
| + | |||
| + | |||
| + | ==Technische Informationen== | ||
| + | ===I2C-Adressen=== | ||
| + | '''I2C-Adressen der MultiIO:''' | ||
| + | {| {{Blauetabelle}} | ||
| + | |- | ||
| + | | '''I2C-Baustein''' || '''I2C-<br/>Adresse *''' || '''Alternative I2C-Adressen''' || '''Verwendung''' | ||
| + | |- | ||
| + | | I2C Voltage & Current Sensor (LTC2990) || 0x98 ² || 0x9a ², 0x9c, 0x9e, 0xee || Voltage & Current Sensor | ||
| + | |- | ||
| + | | I2C Real Time Clock (RTC DS1307Z) || 0xd0 ² || || Real Time Clock | ||
| + | |- | ||
| + | | I2C Temperature Sensor (TCN75A) || 0x90 ² || 0x92 ², 0x98 ², 0x9a ² || Temperature Sensor | ||
| + | |- | ||
| + | | I2C Servo Controller (PCA9685) || 0x80 || 0x82, 0x84, 0x86, 0xe0 ² || Servo Controller | ||
| + | |- | ||
| + | | I2C EEPROM (24LCXXX) || 0xa0 ² || 0xa2 ², 0xa4 ², 0xa6 ², 0xa8, 0xaa, 0xac, 0xae || EEPROM | ||
| + | |- | ||
| + | |} | ||
| + | '''Zu *)''' Standard-Adresse. | ||
| + | |||
| + | '''Zu ²)''' Adress-Konflikt mit dem RP6-System möglich (siehe [[RP6_-_Programmierung#RP6_I2C-Adressen|hier]])! | ||
| + | |||
| + | ===Verschaltung der Jumperblöcke=== | ||
| + | Auf der MultiIO Zusatzplatine für den RP6(v2) gibt es vier Jumperblöcke (J_IO, J_ADC, J_ADIO, J_WAHL), auf die je nach angeschlossenem Prozessorsystem (M32, M128, M256) und je nach Konfiguration Steckbrücken aufgesteckt werden müssen. Die Lage dieser Steckbrücken (Jumper) wurde in den vorangegangenen Kapiteln beschrieben. Wenn man selbst prüfen möchte, ob die eigene Konfiguration stimmt, fehlt häufig die Information, wie die Jumperblöcke untereinander und mit den Anschlußsteckern (IO-Mxxx, ADC-Mxxx, ADC-M256) verschaltet sind. Außer diesen Jumperblöcken sind noch die Stiftleisten für den Anschluß von GPS (5-polige Stiftleiste mit 5V/2xGND/Tx/Rx und 3-polige Stiftleiste mit GND/Tx/Rx5V), DCF77, Bumper, CNY70 (LFS), Snake, LDRs, Gyro & Kompass abgebildet, '''auf die natürlich keine Steckbrücken gesteckt werden dürfen'''. | ||
| + | Die Verschaltung dieser Komponenten zeigt die folgende Abbildung: | ||
| + | |||
| + | [[Bild:MultiIO-Jumperbloecke.PNG|Jumperblöcke der MultiIO]] | ||
| + | |||
| + | ====Beispiel 3,3V-Messung==== | ||
| + | Wie kann man aus diesem "Wirrwarr" von (türkisfarbenen) Verbindungen auf der MultiIO Hinweise auf eine eigene Konfiguration bzw. auf die Position von Jumpern bekommen? | ||
| + | |||
| + | Am Beispiel der '''3,3V-Messung''' soll das einmal verdeutlicht werden: | ||
| + | |||
| + | Auf der MultiIO wird ja u.a. auch vom IC MIC5219-3.3 eine 3,3V-Spannung erzeugt, die für 3,3V-Sensoren (z.B. I2C-Luftdrucksensor BMP085) genutzt werden kann. Diese Spannung kann man (zu Kontrollzwecken) auch messen. Die folgende Abbildung zeigt die (hellgrünen) Verbindungen zwischen den Jumperblöcken, die hierfür eine Rolle spielen: | ||
| + | |||
| + | [[Bild:MultiIO-Jumperbloecke_3V3.PNG|MultiIO Jumperblöcke: Verschaltung der 3,3V-Messung]] | ||
| + | |||
| + | Verfolgt man die hellgrünen 3,3V-Mess-Verbindungen im Bild von oben nach unten, dann wird die auf der MultiIO erzeugte 3,3V-Spannung (über einen Widerstand R36) zum Pin "3V3" des Wahl-Jumper-Blocks (J_WAHL) geführt. Von dort gelangt sie direkt zu zwei Pins des Jumperblocks J_ADC (beschriftet mit "3V3?"). Setzt man auf J_ADC ganz links ("Pins 2") einen Jumper, dann endet die 3,3V-Mess-Verbindung an Pin 2 des ADC-Mxxx Steckers. Setzt man stattdessen den Jumper auf J_ADC eine Position weiter nach rechts ("Pins 9"), dann endet die Mess-Verbindung an Pin 9 des ADC-Mxxx Steckers. Für diese beiden Verbindungen ist kein Jumper auf J_WAHL erforderlich. | ||
| + | |||
| + | Durch einen Jumper auf die Pins "3V3" und "AD3" von J_WAHL (Standard-Stellung der Jumper!) wird die 3,3V-Mess-Verbindung zu Pin 3 von J_ADC und von dort weiter zu Pin 3 des ADC-Mxxx Steckers geführt. | ||
| + | |||
| + | Steckt man den Jumper von J_WAHL auf die Pins "PF4" und "3V3" um, dann gelangt die 3,3V-Mess-Leitung zu Pin "SnT PF4" des J_ADIO Jumperblocks und von dort (falls ein Jumper auf die Pins in Position 4 von links von J_ADIO gesteckt wird) zu Pin 9 des ADC-M256 Steckers. | ||
| + | |||
| + | Das bedeutet, dass die 3,3V-Mess-Spannung der MultiIO durch Umstecken von Jumpern zu insgesamt ''vier Positionen'' geführt werden kann: | ||
| + | * ADC-Mxxx Stecker Pin 2 | ||
| + | * ADC-Mxxx Stecker Pin 9 | ||
| + | * ADC-Mxxx Stecker Pin 3 | ||
| + | * ADC-M256 Stecker Pin 9 | ||
| + | Wenn man nun herausfinden will, mit welchem Portpin bzw. ADC-Kanal die 3,3V-Spannung gemessen werden kann, dann geben die ''Tabellen'': | ||
| + | * M256 -> [[RP6_Multi_IO_Projekt_-_Software#RP6_M256_WIFI|Stecker ADC-Mxxx und Stecker ADC-M256]] | ||
| + | * M128 -> [[RP6_Multi_IO_Projekt_-_Software#RP6_CCPRO_M128|Stecker ADC-Mxxx]] | ||
| + | * M32 -> [[RP6_Multi_IO_Projekt_-_Software#RP6_CONTROL_M32|Stecker ADC-Mxxx]] | ||
| + | ... darüber Auskunft. | ||
| + | Nehmen wir an, die M256 ist an die MultiIO angeschlossen: | ||
| + | |||
| + | Dann entsprechen die Pins 3 und 9 des ADC-Mxxx Steckers den ADC-Kanälen 9 und 12 der M256. Der Pin 2 des ADC-Mxxx Steckers entspricht bei der M256 GND und ist nicht zur 3,3V-Messung nutzbar. Am ADC-M256 Stecker entspricht der Pin 9 dem ADC-Kanal 4 der M256. | ||
| + | |||
| + | Im Ergebnis ist also die 3,3V-Spannung auf der MultiIO abhängig von der Jumperstellung mit den ADC-Kanälen 4, 9 und 12 der M256 messbar. | ||
| + | |||
| + | Wenn man sich für eine bestimmte Jumperstellung entschieden hat, muss man u.U. noch der zugehörigen Library mitteilen, auf welchem ADC-Kanal nun gemessen werden kann. Die Anpassung muss (z.B. für die M256) im zugehörigen [[RP6_Multi_IO_Projekt_-_Software#Configuration_Header|Configuration Header]] (Datei RP6M256_MultiIO.h) erfolgen: | ||
| + | |||
| + | // 3V3 Voltage Sensor: | ||
| + | #define ADC_MULTIIO_3V3 ADC_9 // At ADC-Mxxx | ||
| + | //#define ADC_MULTIIO_3V3 ADC_12 // At ADC-Mxxx | ||
| + | //#define ADC_MULTIIO_3V3 ADC_4 // At ADC-M256 | ||
| + | |||
| + | Hier ist DIE Definition zu wählen, die zur Jumperstellung passt. Nach dem Neukompilieren der Library wird die Änderung wirksam. | ||
| + | |||
| + | Bei der Auswahl von Portpins für eine bestimmte Funktion der MultiIO ist Folgendes unbedingt zu beachten: | ||
| + | |||
| + | * Man sollte sich vergewissern, dass der gewählte Portpin (bzw. ADC-Kanal) nicht schon für eine andere Funktion benutzt wird. | ||
| + | * Man sollte sicher stellen, dass die gewünschte Funktion (z.B. ein ADC-Kanal) tatsächlich an dem festgelegten Pin des IO- oder ADC-Mxxx Steckers vorhanden ist. | ||
| + | * Es ist zu empfehlen, sich an die Standard-Stellung der Jumper zu halten, weil eine Änderung möglicherweise zu Problemen führt, wenn unterschiedliche Programme die veränderte Library nutzen: Man muss dann gut die Verwendung der Portpins dokumentieren. | ||
| + | |||
| + | So weit das Beispiel der 3,3V-Messung mit der M256. Auf die selbe Weise kann man die weiteren Ein-/Ausgabe-Funktionen der MultiIO mit ihren Jumperstellungen auch für die anderen Prozessorsysteme (M128, M32) nachvollziehen. | ||
| + | <br/> | ||
==Siehe auch== | ==Siehe auch== | ||
| Zeile 5.201: | Zeile 9.500: | ||
* [[RP6 - Programmierung]] | * [[RP6 - Programmierung]] | ||
* [[RP6v2]] | * [[RP6v2]] | ||
| + | * [[RP6 Sensor Board und Xtra Module]] | ||
* [[RP6 Kamera - Mitmach-Projekt]] | * [[RP6 Kamera - Mitmach-Projekt]] | ||
| + | * [[RP6 - Morse-Code]] | ||
* [[RP6v2 I2C-Portexpander]] | * [[RP6v2 I2C-Portexpander]] | ||
* [[RP6v2 USB-RS232-Adapter]] | * [[RP6v2 USB-RS232-Adapter]] | ||
* [[RP6v2 Orientierung]] | * [[RP6v2 Orientierung]] | ||
* [[RP6 Multi IO Projekt]] | * [[RP6 Multi IO Projekt]] | ||
| + | * [[RP6 ArduIO]] | ||
| + | * [[RP6 ArduIO - Software]] | ||
* [[CCRP5]] | * [[CCRP5]] | ||
* [[Yeti]] | * [[Yeti]] | ||
| Zeile 5.211: | Zeile 9.514: | ||
* [[C't-Bot]] | * [[C't-Bot]] | ||
| + | <br/> | ||
==Weblinks== | ==Weblinks== | ||
| − | + | Umfangreiche Dokumentation, sämtliche Beispielprogramme und Bibliotheken, Daten, Teilelisten und eine Lötanleitung finden sich [https://www.dropbox.com/sh/7fab3gyweezdrcy/l4uw9djFxP HIER]. | |
[http://www.roboternetz.de/community/threads/40090-RP6Control-M32-Library-f%C3%BCr-8-Servos?highlight=servo+lib Servo-Lib für M32 von Dirk] | [http://www.roboternetz.de/community/threads/40090-RP6Control-M32-Library-f%C3%BCr-8-Servos?highlight=servo+lib Servo-Lib für M32 von Dirk] | ||
| Zeile 5.228: | Zeile 9.532: | ||
==Autoren== | ==Autoren== | ||
| − | --[ | + | --[http://www.roboternetz.de/community/members/1972-Dirk Dirk] 10:41, 13. Jan 2015 (CET) |
| − | --[ | + | --[http://www.roboternetz.de/community/members/47148-fabqu fabqu] 21:30, 9. Mär 2013 (CET) |
[[Kategorie:Elektronik]] | [[Kategorie:Elektronik]] | ||
[[Kategorie:Projekte]] | [[Kategorie:Projekte]] | ||
[[Kategorie:Software]] | [[Kategorie:Software]] | ||
Aktuelle Version vom 11. Januar 2016, 21:24 Uhr
Inhaltsverzeichnis
- 1 RP6 Multi IO Erweiterungsmodul
- 1.1 Hardware
- 1.2 Software
- 1.3 Technische Informationen
- 1.4 Siehe auch
- 1.5 Weblinks
- 1.6 Autoren
RP6 Multi IO Erweiterungsmodul
Das RP6-Multi-IO Erweiterungsmodul besteht aus fünf Platinen. Eine Hauptplatine, im üblichen RP6-Layout, enthält eine Vielzahl möglicher Sensoren und Aktoren wie Temperatursensor, Stromsensor, Berührungssensor, Servo-Ansteuerung, LEDs, Buzzer etc. An sie können die vier weiteren Platinen angesteckt werden. Diese sind eine Bumper-Platine zum Anbringen am Heck des RP6, eine Radio-Platine, eine Platine mit vier Tastern und eine mit einem Liniensensor-Array von fünf CNY70-Reflexoptokopplern.
Diese fünf Platinen können bei fabqu (oder [1]) bestellt werden. Sie sind mit Lötstopplack (schwarz) sowie beidseitig mit einem Bestückungsdruck (in weiß) versehen. Sie sind nicht aufgebaut, aber fabqu bietet an, die schwer einzulötenden SMD-ICs (und bei Bedarf auch alle anderen SMD-Bauteile) in einer einmaligen Sammelbestellung zu bestellen und einzulöten.
Umfangreiche Dokumentation, sämtliche Beispielprogramme und Bibliotheken, Daten, Teilelisten und eine Lötanleitung finden sich in den Weblinks.
Hardware
Der Hardware-Artikel befindet sich HIER.
Software
In diesem Artikel, der die Software für die tolle von Fabian designte Multi IO Projekt Platine vorstellt, darf ich (Dirk) mich austoben. Die vorgestellten Bibliotheken sind natürlich nur ein Vorschlag und sollen nur zur Anregung eigener Projekte und eigener Bibliotheken dienen. Es ist geplant, eine Bibliothek mit allen fest installierten Sensoren und Aktoren aufzubauen. Darüber hinaus soll sich eine weitere Bibliothek mit Umwelt-Fragen beschäftigen (Luftdruck, Luftfeuchtigkeit, Temperatur) sowie eine mit den Sensoren für Lageerkennung (GPS, 2D- oder 9D-Kompass). Diese Bibliotheken liegen – wie das gesamte Projekt – als Open Source vor und werden den Usern zugänglich gemacht.
RP6 M256 WIFI
Die M256-WiFi-Platine (= "M256") kann über den I2C-Bus alle I2C-Sensoren und I2C-Aktoren ansteuern und hat mit ihren vielen freien Ressourcen (Portpins) natürlich optimale Voraussetzungen, um die weiteren Funktionen der Multi IO Platine zu nutzen. Dazu wird sie (wenn alle Funktionen gleichzeitig genutzt werden sollen) mit drei Steckverbindungen angeschlossen:
- IO-Mxxx <-> IO_PWM/T2/T3
- ADC-Mxxx <-> ADC_IO2/CMP
- ADC-M256 <-> ADC_IO1
Dadurch ergeben sich umfangreiche Möglichkeiten, die Sensoren der Multi IO Platine auszulesen und Aktoren zu schalten. Die folgenden 3 Tabellen zeigen die Verbindungen:
Stecker IO-Mxxx:
| Stecker-Pin | M256-Port | M256-Funktion | Multi IO-Funktion | I/O |
| 1 | PE7 | ICP3 | DCF77 | I |
| 2 | GND | |||
| 3 | PE6 | T3 | SHARPS_PWR | O |
| 4 | PE5 | OC3C | SNAKE_SWITCH | O |
| 5 | PB4 | OC2A | BUZZER | O |
| 6 | PK7 | ADC15 | (TX *) | |
| 7 | PH6 | OC2B | SNAKE_KEY / (TX *) | I |
| 8 | PK6 | ADC14 | (RX *) | |
| 9 | PE4 | OC3B | LFS_PWR | O |
| 10 | VDD |
Zu *) UART nicht am IO_PWM/Tx/Ty Stecker der M256 vorhanden!
Stecker ADC-Mxxx:
| Stecker-Pin | M256-Port | M256-Funktion | Multi IO-Funktion | I/O |
| 1 | PK0 | ADC8 | TOUCH / LFS_L | I |
| 2 | GND | |||
| 3 | PK1 | ADC9 | 3V3 / LFS_M | I |
| 4 | PE3 | OC3A/AIN1 | BUMPER_L | I/O * |
| 5 | PK2 | ADC10 | LFS_R | I |
| 6 | GND | |||
| 7 | PK3 | ADC11 | BUTTONS | I |
| 8 | PE2 | XCK0/AIN0 | BUMPER_R | I/O * |
| 9 | PK4 | ADC12 | 3V3 alt.1 | I |
| 10 | VDD |
Zu *) Eingang für Bumper-Taster, Ausgang für Bumper-LED!
Stecker ADC-M256:
| Stecker-Pin | M256-Port | M256-Funktion | Multi IO-Funktion | I/O |
| 1 | PF1 | ADC1 | LDR1 | I |
| 2 | GND | GND | ||
| 3 | PF0 | ADC0 | LDR2 | I |
| 4 | VDD | |||
| 5 | PF2 | ADC2 | LFS_R1 | I |
| 6 | GND | GND | ||
| 7 | PF3 | ADC3 | LFS_L1 | I |
| 8 | GND | GND | ||
| 9 | PF4 | ADC4 | SHARP_L / 3V3 | I |
| 10 | GND | GND | ||
| 11 | PF5 | ADC5 | SHARP_R / TOUCH | I |
| 12 | PF6 | ADC6 | SNAKE_R | I |
| 13 | PF7 | ADC7 | SNAKE_L | I |
| 14 | GND | GND |
Multi IO Projekt Library
Diese Avr-gcc Library für das Multi IO Projekt Board (= "MultiIO") geht von folgenden Voraussetzungen aus:
- Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt (1).
- Die M256 ist der I2C-Bus Master.
- Die I2C-Bus Geschwindigkeit beträgt 100 kHz.
- Alle Hardware-Komponenten der MultiIO sind aufgebaut (2).
- Alle Jumper auf der MultiIO sind in ihrer Standardstellung (3).
- Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden.
- Der Wannenstecker IO_Mxxx der MultiIO ist mit dem Wannenstecker IO_PWM/T2/T3 der M256 1:1 verbunden.
- Der Wannenstecker ADC_Mxxx der MultiIO ist mit dem Wannenstecker ADC_IO2/CMP der M256 1:1 verbunden.
- Der Wannenstecker ADC_M256 der MultiIO ist mit dem Wannenstecker ADC_IO1 der M256 1:1 verbunden.
Zu (1): Dies soll nur der 1. Schritt sein. Natürlich ist die MultiIO auch
an die RP6 BASE, die M32 und die M128 des RP6-Systems und an
praktisch alle anderen Microcontroller-Plattformen anschließbar!
Zu (2): Wenn nicht alle Komponenten aufgebaut sind, sind die zugehörigen
Funktionen natürlich nicht funktionsfähig und können nicht benutzt
werden.
Zu (3): Siehe folgende Abbildung!
Standard-Jumperstellung für die M256
Hinweis: Die Jumper sind orange eingezeichnet!
Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!! Die Jumper der Stromversorgung wurden nicht berücksichtigt!
Die Library (Software-Bibliothek) besteht aus drei Teilen:
- Dem Configuration Header -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss.
- Dem Library Header -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library.
- Der Library Source -> Das ist die eigentliche Library.
Configuration Header
Datei RP6M256_MultiIO.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples * **************************************************************************** * Example: RP6M256 MultiIO Library * Author(s): Dirk * **************************************************************************** * Description: * Configuration header file for new MultiIO Project Board library. * * **************************************************************************** */ #ifndef RP6M256_MULTIIO_H #define RP6M256_MULTIIO_H /*****************************************************************************/ // MultiIO hardwired components: // - I2C Voltage & Current Sensor (LTC2990) // - I2C Real Time Clock (RTC DS1307Z) // - I2C Temperature Sensor (TCN75A) // - I2C Servo Controller (PCA9685) // - I2C EEPROM (24LCXXX) // - 3V3 Voltage Sensor // - Touch Sensor (with NE555) // - Buttons // - LEDs // - Buzzer /*****************************************************************************/ // I2C Voltage & Current Sensor (LTC2990): #define I2C_MULTIIO_VCS_ADR 0x98 // ADR1/0 = 0/0 //#define I2C_MULTIIO_VCS_ADR 0x9a // ADR1/0 = 0/1 //#define I2C_MULTIIO_VCS_ADR 0x9c // ADR1/0 = 1/0 //#define I2C_MULTIIO_VCS_ADR 0x9e // ADR1/0 = 1/1 //#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address // (Shunt resistor, current and voltage adjustment!) #define SHUNT_R 0.051 // R10 [Ohm] #define C_ADJUST 2.0 // Current adjust factor #define C_OFFSET 0.0 // Current offset [mA] #define VBAT_ADJUST 3.2 // (R13 + R14) / R14 #define VSERVO_ADJUST 2.5 // (R32 + R22) / R22 /*****************************************************************************/ // I2C Real Time Clock (RTC DS1307Z): #define I2C_MULTIIO_RTC_ADR 0xD0 // Default /*****************************************************************************/ // I2C Temperature Sensor (TCN75A): // (A1 always 0!) #define I2C_MULTIIO_TEMP_ADR 0x90 // A2/0 = 0/0 //#define I2C_MULTIIO_TEMP_ADR 0x92 // A2/0 = 0/1 //#define I2C_MULTIIO_TEMP_ADR 0x98 // A2/0 = 1/0 //#define I2C_MULTIIO_TEMP_ADR 0x9a // A2/0 = 1/1 /*****************************************************************************/ // I2C Servo Controller (PCA9685): // (A5, A4, A3, A2 always 0!) #define I2C_MULTIIO_SERVO_ADR 0x80 // A1/0 = 0/0 //#define I2C_MULTIIO_SERVO_ADR 0x82 // A1/0 = 0/1 //#define I2C_MULTIIO_SERVO_ADR 0x84 // A1/0 = 1/0 //#define I2C_MULTIIO_SERVO_ADR 0x86 // A1/0 = 1/1 //#define I2C_MULTIIO_SERVO_ADR 0xe0 // ALLCALLADR // (Servo power is connected to LED8 of the PCA9685!) #define CHSERVOPWR 9 // Servo left touch (LT), right touch (RT), middle position (MP) constants: // (Hints: - Servo impulse length [ms] = Servo position value / 204.8 // (Formula only valid for a PWM of 50 Hz!) // - Min. servo impulse (0,7 ms) = Servo position 143 // - Mid. servo impulse (1,5 ms) = Servo position 307 // - Max. servo impulse (2,3 ms) = Servo position 471 // - !!! You should NOT use servo position values < 143 or > 471 !!!) #define SERVO1_LT 205 // Servo impulse ~1ms #define SERVO1_RT 410 // Servo impulse ~2ms #define SERVO1_MP ((SERVO1_RT - SERVO1_LT) / 2 + SERVO1_LT) #define SERVO2_LT 205 #define SERVO2_RT 410 #define SERVO2_MP ((SERVO2_RT - SERVO2_LT) / 2 + SERVO2_LT) #define SERVO3_LT 205 #define SERVO3_RT 410 #define SERVO3_MP ((SERVO3_RT - SERVO3_LT) / 2 + SERVO3_LT) #define SERVO4_LT 205 #define SERVO4_RT 410 #define SERVO4_MP ((SERVO4_RT - SERVO4_LT) / 2 + SERVO4_LT) #define SERVO5_LT 205 #define SERVO5_RT 410 #define SERVO5_MP ((SERVO5_RT - SERVO5_LT) / 2 + SERVO5_LT) #define SERVO6_LT 205 #define SERVO6_RT 410 #define SERVO6_MP ((SERVO6_RT - SERVO6_LT) / 2 + SERVO6_LT) #define SERVO7_LT 205 #define SERVO7_RT 410 #define SERVO7_MP ((SERVO7_RT - SERVO7_LT) / 2 + SERVO7_LT) #define SERVO8_LT 205 #define SERVO8_RT 410 #define SERVO8_MP ((SERVO8_RT - SERVO8_LT) / 2 + SERVO8_LT) /*****************************************************************************/ // I2C EEPROM (24LCXXX): // (A2=1 not usable with 24LC1024-P!) #define I2C_MULTIIO_EEPROM_ADR 0xA0 // A2/1/0 = 0/0/0 //#define I2C_MULTIIO_EEPROM_ADR 0xA2 // A2/1/0 = 0/0/1 //#define I2C_MULTIIO_EEPROM_ADR 0xA4 // A2/1/0 = 0/1/0 //#define I2C_MULTIIO_EEPROM_ADR 0xA6 // A2/1/0 = 0/1/1 //#define I2C_MULTIIO_EEPROM_ADR 0xA8 // A2/1/0 = 1/0/0 //#define I2C_MULTIIO_EEPROM_ADR 0xAA // A2/1/0 = 1/0/1 //#define I2C_MULTIIO_EEPROM_ADR 0xAC // A2/1/0 = 1/1/0 //#define I2C_MULTIIO_EEPROM_ADR 0xAE // A2/1/0 = 1/1/1 // I2C-EEPROM storage capacity [kbit]: #define I2C_EEPROM_KBIT 32 // 24LC32-P <== Default //#define I2C_EEPROM_KBIT 64 // 24LC64-P //#define I2C_EEPROM_KBIT 128 // 24LC128-P //#define I2C_EEPROM_KBIT 256 // 24LC256-P //#define I2C_EEPROM_KBIT 512 // 24LC512-P //#define I2C_EEPROM_KBIT 1024 // 24LC1024-P // I2C-EEPROM pagesize [byte]: // ATTENTION: The pagesize must fit to the EEPROM type defined above! #define I2C_EEPROM_PAGESIZE 32 // EEPROM 32 or 64 kbit //#define I2C_EEPROM_PAGESIZE 64 // EEPROM 128 or 256 kbit //#define I2C_EEPROM_PAGESIZE 128 // EEPROM 512 kbit //#define I2C_EEPROM_PAGESIZE 256 // EEPROM 1024 kbit /*****************************************************************************/ // 3V3 Voltage Sensor: #define ADC_MULTIIO_3V3 ADC_9 // At ADC-Mxxx //#define ADC_MULTIIO_3V3 ADC_12 // At ADC-Mxxx //#define ADC_MULTIIO_3V3 ADC_4 // At ADC-M256 /*****************************************************************************/ // Touch Sensor (with NE555): #define ADC_MULTIIO_TOUCH ADC_8 // At ADC-Mxxx //#define ADC_MULTIIO_TOUCH ADC_5 // At ADC-M256 #define ADCVAL_NOTOUCH 15 #define ADCVAL_TOUCH 1020 #define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) /*****************************************************************************/ // Buttons: #define ADC_MULTIIO_BUTTONS ADC_11 // At ADC-Mxxx #define ADCVAL_BUTTON1 14 #define ADCVAL_BUTTON2 577 #define ADCVAL_BUTTON3 736 #define ADCVAL_BUTTON4 812 #define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) #define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) #define ADCVAL_LIMIT34 ((ADCVAL_BUTTON4 - ADCVAL_BUTTON3) / 2 + ADCVAL_BUTTON3) #define ADCVAL_LIMIT40 ((1023 - ADCVAL_BUTTON4) / 2 + ADCVAL_BUTTON4) /*****************************************************************************/ // LEDs: // (Status LED1..LED4 are connected to LED15..LED12 of the PCA9685!) #define CHLED1 16 #define CHLED2 15 #define CHLED3 14 #define CHLED4 13 /*****************************************************************************/ // Buzzer: // (This library assumes that the buzzer is connected to OC2A_PI4 (PB4). // This is the case, if you connect IO-Mxxx with IO_PWM/T2/T3 of the M256. // // Possible connections of the buzzer on the MultiIO Project Board: // // M256 plug: Portpin name: Portpin: // IO_PWM/T2/T3 OC2A_PI4 PB4 <== Default of THIS library! // IO_PWM/T0/T1 IO_OC0B PG5 // UART_SPI1/T5 IO_PL3_OC5A PL3 // UART_SPI2/T4 IO_PH5_OC4C PH5 // // You may of course use one of the other portpins (PG5, PL3, PH5) for the // buzzer, but then you will have to write your own buzzer functions!!! // Attention: Timer/Counter0 is used by the RP6M256 library, so you may NOT // use IO_OC0B (PG5) with hardware PWM!) /*****************************************************************************/ // Other ADC channel definitions: // (Depending on jumper settings on the MultiIO Project Board!) #define ADC_MULTIIO_LDR2 ADC_0 // At ADC-M256 #define ADC_MULTIIO_LDR1 ADC_1 // At ADC-M256 #define ADC_MULTIIO_LFS_R1 ADC_2 // At ADC-M256 #define ADC_MULTIIO_LFS_L1 ADC_3 // At ADC-M256 #define ADC_MULTIIO_SHARP_L ADC_4 // At ADC-M256 #define ADC_MULTIIO_SHARP_R ADC_5 // At ADC-M256 #define ADC_MULTIIO_SNAKE_R ADC_6 // At ADC-M256 #define ADC_MULTIIO_SNAKE_L ADC_7 // At ADC-M256 #define ADC_MULTIIO_LFS_L ADC_8 // At ADC-Mxxx #define ADC_MULTIIO_LFS_M ADC_9 // At ADC-Mxxx #define ADC_MULTIIO_LFS_R ADC_10 // At ADC-Mxxx // Other IO portpin definitions: // (Bumper/LEDs in-/outputs at the ADC-Mxxx plug!) #define IO_MULTIIO_BUMPER_R_IN IO_PE2_XCK0_AIN0 // At ADC-Mxxx #define IO_MULTIIO_BUMPER_R_DDR DDRE #define IO_MULTIIO_BUMPER_R_PIN PINE #define IO_MULTIIO_BUMPER_R_PORT PORTE #define IO_MULTIIO_BUMPER_L_IN IO_PE3_OC3A_AIN1 // At ADC-Mxxx #define IO_MULTIIO_BUMPER_L_DDR DDRE #define IO_MULTIIO_BUMPER_L_PIN PINE #define IO_MULTIIO_BUMPER_L_PORT PORTE // Other IO portpin definitions: // (Depending on the IO-Mxxx plug connection. Default: IO_PWM/T2/T3!) #define IO_MULTIIO_LFS_PWR_IN IO_PE4_OC3B_I4 // IO-Mxxx: IO_PWM/T2/T3 #define IO_MULTIIO_LFS_PWR_DDR DDRE #define IO_MULTIIO_LFS_PWR_PORT PORTE //#define IO_MULTIIO_LFS_PWR_IN OC1B_PI6 // IO-Mxxx: IO_PWM/T0/T1 //#define IO_MULTIIO_LFS_PWR_DDR DDRB //#define IO_MULTIIO_LFS_PWR_PORT PORTB //#define IO_MULTIIO_LFS_PWR_IN IO_PL4_OC5B // IO-Mxxx: UART_SPI1/T5 //#define IO_MULTIIO_LFS_PWR_DDR DDRL //#define IO_MULTIIO_LFS_PWR_PORT PORTL //#define IO_MULTIIO_LFS_PWR_IN IO_PH3_OC4A // IO-Mxxx: UART_SPI2/T4 //#define IO_MULTIIO_LFS_PWR_DDR DDRH //#define IO_MULTIIO_LFS_PWR_PORT PORTH #define IO_MULTIIO_SHARPS_PWR_IN IO_PE6_T3_I6 // IO-Mxxx: IO_PWM/T2/T3 #define IO_MULTIIO_SHARPS_PWR_DDR DDRE #define IO_MULTIIO_SHARPS_PWR_PORT PORTE //#define IO_MULTIIO_SHARPS_PWR_IN IO_PD6_T1 // IO-Mxxx: IO_PWM/T0/T1 //#define IO_MULTIIO_SHARPS_PWR_DDR DDRD //#define IO_MULTIIO_SHARPS_PWR_PORT PORTD //#define IO_MULTIIO_SHARPS_PWR_IN IO_PL2_T5 // IO-Mxxx: UART_SPI1/T5 //#define IO_MULTIIO_SHARPS_PWR_DDR DDRL //#define IO_MULTIIO_SHARPS_PWR_PORT PORTL //#define IO_MULTIIO_SHARPS_PWR_IN IO_PH7_T4 // IO-Mxxx: UART_SPI2/T4 //#define IO_MULTIIO_SHARPS_PWR_DDR DDRH //#define IO_MULTIIO_SHARPS_PWR_PORT PORTH #define IO_MULTIIO_SNAKE_SWITCH_IN IO_PE5_OC3C_I5 // IO-Mxxx: IO_PWM/T2/T3 #define IO_MULTIIO_SNAKE_SWITCH_DDR DDRE #define IO_MULTIIO_SNAKE_SWITCH_PORT PORTE //#define IO_MULTIIO_SNAKE_SWITCH_IN OC0A_OCM_PI7 // IO-Mxxx: IO_PWM/T0/T1 //#define IO_MULTIIO_SNAKE_SWITCH_DDR DDRB //#define IO_MULTIIO_SNAKE_SWITCH_PORT PORTB //#define IO_MULTIIO_SNAKE_SWITCH_IN IO_PD5_XCK1 // IO-Mxxx: UART_SPI1/T5 //#define IO_MULTIIO_SNAKE_SWITCH_DDR DDRD //#define IO_MULTIIO_SNAKE_SWITCH_PORT PORTD //#define IO_MULTIIO_SNAKE_SWITCH_IN IO_PH2_XCK2 // IO-Mxxx: UART_SPI2/T4 //#define IO_MULTIIO_SNAKE_SWITCH_DDR DDRH //#define IO_MULTIIO_SNAKE_SWITCH_PORT PORTH #define IO_MULTIIO_SNAKE_KEY_IN IO_PH6_OC2B // IO-Mxxx: IO_PWM/T2/T3 #define IO_MULTIIO_SNAKE_KEY_DDR DDRH #define IO_MULTIIO_SNAKE_KEY_PIN PINH //#define IO_MULTIIO_SNAKE_KEY_IN OC1A_PI5 // IO-Mxxx: IO_PWM/T0/T1 //#define IO_MULTIIO_SNAKE_KEY_DDR DDRB //#define IO_MULTIIO_SNAKE_KEY_PIN PINB //#define IO_MULTIIO_SNAKE_KEY_IN IO_PL5_OC5C // IO-Mxxx: UART_SPI1/T5 //#define IO_MULTIIO_SNAKE_KEY_DDR DDRL //#define IO_MULTIIO_SNAKE_KEY_PIN PINL //#define IO_MULTIIO_SNAKE_KEY_IN IO_PH4_OC4B // IO-Mxxx: UART_SPI2/T4 //#define IO_MULTIIO_SNAKE_KEY_DDR DDRH //#define IO_MULTIIO_SNAKE_KEY_PIN PINH #define IO_MULTIIO_DCF77_IN IO_PE7_ICP3_I7 // IO-Mxxx: IO_PWM/T2/T3 #define IO_MULTIIO_DCF77_DDR DDRE #define IO_MULTIIO_DCF77_PIN PINE //#define IO_MULTIIO_DCF77_IN IO_PD4_ICP1 // IO-Mxxx: IO_PWM/T0/T1 //#define IO_MULTIIO_DCF77_DDR DDRD //#define IO_MULTIIO_DCF77_PIN PIND //#define IO_MULTIIO_DCF77_IN IO_PL1_ICP5 // IO-Mxxx: UART_SPI1/T5 //#define IO_MULTIIO_DCF77_DDR DDRL //#define IO_MULTIIO_DCF77_PIN PINL //#define IO_MULTIIO_DCF77_IN IO_PL0_ICP4 // IO-Mxxx: UART_SPI2/T4 //#define IO_MULTIIO_DCF77_DDR DDRL //#define IO_MULTIIO_DCF77_PIN PINL /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_MultiIOLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Header
Datei RP6M256_MultiIOLib.h:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO Library
* Author(s): Dirk
* ****************************************************************************
* Description:
* Header file for new MultiIO Project Board library.
*
* ****************************************************************************
*/
#ifndef RP6M256_MULTIIOLIB_H
#define RP6M256_MULTIIOLIB_H
/*****************************************************************************/
// MultiIO hardwired components:
// - I2C Voltage & Current Sensor (LTC2990)
// - I2C Real Time Clock (RTC DS1307Z)
// - I2C Temperature Sensor (TCN75A)
// - I2C Servo Controller (PCA9685)
// - I2C EEPROM (24LCXXX)
// - 3V3 Voltage Sensor
// - Touch Sensor (with NE555)
// - Buttons
// - LEDs
// - Buzzer
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6M256_MultiIO.h"
/*****************************************************************************/
// I2C Voltage & Current Sensor (LTC2990):
// Registers:
#define LTC2990_STATUS 0
#define LTC2990_CONTROL 1
#define LTC2990_TRIGGER 2
#define LTC2990_TINT_MSB 4
#define LTC2990_TINT_LSB 5
#define LTC2990_V1_MSB 6
#define LTC2990_V1_LSB 7
#define LTC2990_V2_MSB 8
#define LTC2990_V2_LSB 9
#define LTC2990_V3_MSB 10
#define LTC2990_V3_LSB 11
#define LTC2990_V4_MSB 12
#define LTC2990_V4_LSB 13
#define LTC2990_VCC_MSB 14
#define LTC2990_VCC_LSB 15
// Status register bitmasks:
#define LTC2990_STATUS_DEFAULT 0
#define LTC2990_STATUS_BUSY 1
#define LTC2990_STATUS_TINT_READY 2
#define LTC2990_STATUS_V1V2_READY 4
#define LTC2990_STATUS_V2_READY 8
#define LTC2990_STATUS_V3V4_READY 16
#define LTC2990_STATUS_V4_READY 32
#define LTC2990_STATUS_VCC_READY 64
// Control register bitmasks:
#define LTC2990_CONTROL_DEFAULT 0
#define LTC2990_CONTROL_MULTIIO 0b01011010 // Mode V1-V2, V3, V4
// All measurements per mode
// Single acquisition
#define LTC2990_CONTROL_MODE02_DEFAULT 0
#define LTC2990_CONTROL_MODE34_DEFAULT 0
#define LTC2990_CONTROL_REPEAT_SINGLE 64
#define LTC2990_CONTROL_TEMP_FORMAT 128
extern double tint;
extern double cbat;
extern double vbat;
extern double vservo;
extern double vcc;
void LTC2990_write_cfg(uint8_t);
#define LTC2990_init() {LTC2990_write_cfg(LTC2990_CONTROL_MULTIIO);}
void LTC2990_run(void);
void LTC2990_read(void);
void LTC2990_calculate(void);
void LTC2990_measure(void);
/*****************************************************************************/
// I2C Real Time Clock (RTC DS1307Z):
// Registers:
#define DS1307_SECONDS 0
#define DS1307_MINUTES 1
#define DS1307_HOURS 2
#define DS1307_DAY 3
#define DS1307_DATE 4
#define DS1307_MONTH 5
#define DS1307_YEAR 6
#define DS1307_CONTROL 7
#define DS1307_RAM 8
// Control register bitmasks:
#define DS1307_CONTROL_DEFAULT 0
#define DS1307_CONTROL_RS0 1
#define DS1307_CONTROL_RS1 2
#define DS1307_CONTROL_SQWE 16
#define DS1307_CONTROL_OUT 128
enum RTCWEEKDAYS {
R_MO = 1, R_TU, R_WE, R_TH, R_FR, R_SA, R_SU
};
typedef struct {
uint16_t year; // Year
uint8_t month; // Month [1..12]
enum RTCWEEKDAYS weekday; // Weekday [1..7 = R_MO..R_SU]
uint8_t day; // Day [1..31]
} rtcdate_t;
rtcdate_t rtc_date;
typedef struct {
uint8_t dst; // Daylight-saving-time (time zone)
uint8_t hour; // Hour [0..23]
uint8_t minute; // Minute [0..59]
uint8_t second; // Second [0..59]
} rtctime_t;
rtctime_t rtc_time;
uint8_t BCD2DEC(uint8_t);
uint8_t DEC2BCD(uint8_t);
void DS1307_write_cfg(uint8_t);
void DS1307_init(void);
#define CALC_DST // Time zone will be calculated
void DS1307_read(void);
void DS1307_write(void);
uint8_t DS1307_readRAM(uint8_t);
void DS1307_writeRAM(uint8_t, uint8_t);
/*****************************************************************************/
// I2C Temperature Sensor (TCN75A):
// Registers:
#define TCN75_TEMP 0
#define TCN75_CONFIG 1
#define TCN75_HYST 2
#define TCN75_LIMIT 3
// Config register bitmasks:
#define TCN75_CONFIG_RUN 0 // Default
#define TCN75_CONFIG_SHUTDOWN 1
#define TCN75_CONFIG_COMP 0 // Default
#define TCN75_CONFIG_INT 2
#define TCN75_CONFIG_ALERT_LOW 0 // Default
#define TCN75_CONFIG_ALERT_HIGH 4
#define TCN75_CONFIG_FAULT_1 0 // Default
#define TCN75_CONFIG_FAULT_2 8
#define TCN75_CONFIG_FAULT_4 16
#define TCN75_CONFIG_FAULT_6 24
// Only for the TCN75A - high resolution and OneShot mode:
#define TCN75A_CONFIG_RES_9 0 // Default
#define TCN75A_CONFIG_RES_10 32 // 0b00100000
#define TCN75A_CONFIG_RES_11 64 // 0b01000000
#define TCN75A_CONFIG_RES_12 96 // 0b01100000
#define TCN75A_CONFIG_ONESHOT_DISABLED 0 // Default
#define TCN75A_CONFIG_ONESHOT 128 // 0b10000000
extern double temperature;
void TCN75_write_cfg(uint8_t);
#define TCN75_shutdown() {TCN75_write_cfg(TCN75_CONFIG_SHUTDOWN);}
#define TCN75_run(__CONFIG__) {TCN75_write_cfg(__CONFIG__);}
extern uint8_t temperature_low;
extern uint8_t temperature_high;
#define getTemperatureHigh() (temperature_high)
#define getTemperatureLow() (temperature_low)
void TCN75_read(void);
double TCN75_calculate(void);
double TCN75_measure(void);
/*****************************************************************************/
// I2C Servo Controller (PCA9685):
// Registers:
#define PCA9685_MODE1 0
#define PCA9685_MODE2 1
#define PCA9685_SUBADR1 2
#define PCA9685_SUBADR2 3
#define PCA9685_SUBADR3 4
#define PCA9685_ALLCALLADR 5
#define PCA9685_LED0_ON_L 6
#define PCA9685_LED0_ON_H 7
#define PCA9685_LED0_OFF_L 8
#define PCA9685_LED0_OFF_H 9
#define PCA9685_LED1_ON_L 10
#define PCA9685_LED1_ON_H 11
#define PCA9685_LED1_OFF_L 12
#define PCA9685_LED1_OFF_H 13
#define PCA9685_LED2_ON_L 14
#define PCA9685_LED2_ON_H 15
#define PCA9685_LED2_OFF_L 16
#define PCA9685_LED2_OFF_H 17
#define PCA9685_LED3_ON_L 18
#define PCA9685_LED3_ON_H 19
#define PCA9685_LED3_OFF_L 20
#define PCA9685_LED3_OFF_H 21
#define PCA9685_LED4_ON_L 22
#define PCA9685_LED4_ON_H 23
#define PCA9685_LED4_OFF_L 24
#define PCA9685_LED4_OFF_H 25
#define PCA9685_LED5_ON_L 26
#define PCA9685_LED5_ON_H 27
#define PCA9685_LED5_OFF_L 28
#define PCA9685_LED5_OFF_H 29
#define PCA9685_LED6_ON_L 30
#define PCA9685_LED6_ON_H 31
#define PCA9685_LED6_OFF_L 32
#define PCA9685_LED6_OFF_H 33
#define PCA9685_LED7_ON_L 34
#define PCA9685_LED7_ON_H 35
#define PCA9685_LED7_OFF_L 36
#define PCA9685_LED7_OFF_H 37
#define PCA9685_LED8_ON_L 38
#define PCA9685_LED8_ON_H 39
#define PCA9685_LED8_OFF_L 40
#define PCA9685_LED8_OFF_H 41
#define PCA9685_LED9_ON_L 42
#define PCA9685_LED9_ON_H 43
#define PCA9685_LED9_OFF_L 44
#define PCA9685_LED9_OFF_H 45
#define PCA9685_LED10_ON_L 46
#define PCA9685_LED10_ON_H 47
#define PCA9685_LED10_OFF_L 48
#define PCA9685_LED10_OFF_H 49
#define PCA9685_LED11_ON_L 50
#define PCA9685_LED11_ON_H 51
#define PCA9685_LED11_OFF_L 52
#define PCA9685_LED11_OFF_H 53
#define PCA9685_LED12_ON_L 54
#define PCA9685_LED12_ON_H 55
#define PCA9685_LED12_OFF_L 56
#define PCA9685_LED12_OFF_H 57
#define PCA9685_LED13_ON_L 58
#define PCA9685_LED13_ON_H 59
#define PCA9685_LED13_OFF_L 60
#define PCA9685_LED13_OFF_H 61
#define PCA9685_LED14_ON_L 62
#define PCA9685_LED14_ON_H 63
#define PCA9685_LED14_OFF_L 64
#define PCA9685_LED14_OFF_H 65
#define PCA9685_LED15_ON_L 66
#define PCA9685_LED15_ON_H 67
#define PCA9685_LED15_OFF_L 68
#define PCA9685_LED15_OFF_H 69
#define PCA9685_ALL_LED_ON_L 250
#define PCA9685_ALL_LED_ON_H 251
#define PCA9685_ALL_LED_OFF_L 252
#define PCA9685_ALL_LED_OFF_H 253
#define PCA9685_PRE_SCALE 254
#define PCA9685_TESTMODE 255
// Mode1 register bitmasks:
#define PCA9685_MODE1_ALLCALL 1
#define PCA9685_MODE1_SUB3 2
#define PCA9685_MODE1_SUB2 4
#define PCA9685_MODE1_SUB1 8
#define PCA9685_MODE1_SLEEP 16
#define PCA9685_MODE1_AI 32
#define PCA9685_MODE1_EXTCLK 64
#define PCA9685_MODE1_RESTART 128
// Mode2 register bitmasks:
#define PCA9685_MODE2_OUTNE01_DEFAULT 0
#define PCA9685_MODE2_OUTDRV 4
#define PCA9685_MODE2_OCH 8
#define PCA9685_MODE2_INVRT 16
#define F_PCA9685 25000000.0 // Int. Clock: 25 MHz
void PCA9685_init(uint16_t);
#define initServo(__FREQ__) {PCA9685_init(__FREQ__);}
void PCA9685_set(uint8_t, uint16_t);
#define setServo(__SERVO__,__POS__) {PCA9685_set(__SERVO__,__POS__);}
void PCA9685_shutdown(void);
void PCA9685_restart(void);
void setServoPower(uint8_t);
/*****************************************************************************/
// I2C EEPROM (24LCXXX):
uint8_t I2C_EEPROM_readByte(uint16_t memAddr);
void I2C_EEPROM_writeByte(uint16_t memAddr, uint8_t data);
void I2C_EEPROM_readBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length);
void I2C_EEPROM_writeBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length);
/*****************************************************************************/
// 3V3 Voltage Sensor:
extern uint16_t adc3v3;
extern double v3v3;
uint16_t get3V3Sensor(void);
double calculate3V3(void);
double measure3V3(void);
/*****************************************************************************/
// Touch Sensor (with NE555):
extern uint16_t adcTouch;
extern uint8_t touch;
uint8_t getTouch(void);
/*****************************************************************************/
// Buttons:
extern uint16_t adcButtons;
extern uint8_t releasedButtonNumber;
extern uint8_t pressedButtonNumber;
uint8_t getPressedButtonNumber(void);
uint8_t checkPressedButtonEvent(void);
uint8_t checkReleasedButtonEvent(void);
/*****************************************************************************/
// LEDs:
void setMultiIOLEDs(uint8_t leds);
void dimMultiIOLED(uint8_t led, uint16_t duty);
void setMultiIOLED1(uint8_t led);
void setMultiIOLED2(uint8_t led);
void setMultiIOLED3(uint8_t led);
void setMultiIOLED4(uint8_t led);
/*****************************************************************************/
// Buzzer:
// Define tone frequencies (well temperament):
// Great Octave
#define Tone_H 2 // 123Hz
// Small Octave
#define Tone_c 16 // 131Hz
#define Tone_cis 30 // 139Hz
#define Tone_d 42 // 147Hz
#define Tone_dis 54 // 156Hz
#define Tone_e 65 // 165Hz
#define Tone_f 76 // 175Hz
#define Tone_fis 86 // 185Hz
#define Tone_g 96 // 196Hz
#define Tone_gis 105 // 208Hz
#define Tone_a 113 // 220Hz
#define Tone_ais 121 // 233Hz
#define Tone_h 128 // 247Hz
// ' Octave
#define Tone_C1 136 // 262Hz
#define Tone_Cis1 142 // 277Hz
#define Tone_D1 149 // 294Hz
#define Tone_Dis1 155 // 311Hz
#define Tone_E1 160 // 330Hz
#define Tone_F1 166 // 349Hz
#define Tone_Fis1 171 // 370Hz
#define Tone_G1 175 // 392Hz
#define Tone_Gis1 180 // 415Hz
#define Tone_A1 184 // 440Hz
#define Tone_Ais1 188 // 466Hz
#define Tone_H1 192 // 494Hz
// '' Octave
#define Tone_C2 195 // 523Hz
#define Tone_Cis2 199 // 554Hz
#define Tone_D2 202 // 587Hz
#define Tone_Dis2 205 // 622Hz
#define Tone_E2 208 // 659Hz
#define Tone_F2 210 // 698Hz
#define Tone_Fis2 213 // 740Hz
#define Tone_G2 215 // 784Hz
#define Tone_Gis2 217 // 831Hz
#define Tone_A2 219 // 880Hz
#define Tone_Ais2 221 // 932Hz
#define Tone_H2 223 // 988Hz
// ''' Octave
#define Tone_C3 225 // 1047Hz
#define Tone_Cis3 227 // 1109Hz
#define Tone_D3 228 // 1175Hz
#define Tone_Dis3 230 // 1245Hz
#define Tone_E3 231 // 1319Hz
#define Tone_F3 233 // 1397Hz
#define Tone_Fis3 234 // 1480Hz
#define Tone_G3 235 // 1568Hz
#define Tone_Gis3 236 // 1661Hz
#define Tone_A3 237 // 1760Hz
#define Tone_Ais3 238 // 1865Hz
#define Tone_H3 239 // 1976Hz
// '''' Octave
#define Tone_C4 240 // 2093Hz
#define Tone_Cis4 241 // 2217Hz
#define Tone_D4 242 // 2349Hz
#define Tone_Dis4 242 // 2489Hz
#define Tone_E4 243 // 2637Hz
#define Tone_F4 244 // 2794Hz
#define Tone_Fis4 244 // 2960Hz
#define Tone_G4 245 // 3136Hz
#define Tone_Gis4 246 // 3322Hz
#define Tone_A4 246 // 3520Hz
#define Tone_Ais4 247 // 3729Hz
#define Tone_H4 247 // 3951Hz
// ''''' Octave
#define Tone_C5 248 // 4186Hz
void Buzzer_init(void);
void setBuzzerPitch(uint8_t);
void soundBuzzer(uint8_t, uint16_t, uint16_t);
#define buzzer(__PITCH__,__TIME__) {soundBuzzer(__PITCH__,__TIME__,0);}
/*****************************************************************************/
// MultiIO Project Board initialisation and shutdown:
void multiio_init(void);
void multiio_shutdown(void);
/*****************************************************************************/
#endif
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
*
* ---> changes are documented in the file "RP6M256_MultiIOLib.c"
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Library Source
Datei RP6M256_MultiIOLib.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO Library
* Author(s): Dirk
* ****************************************************************************
* Description:
*
* This is our new Library that contains basic routines and functions for
* accessing the hardwired components of the MultiIO Project Board.
*
* There are much more sensors, that may be connected to the MultiIO Project
* Board: Line following sensors (5x CNY70), bumpers, "Snake Vision board",
* LDRs, I2C air pressure sensor, I2C humidity sensor, DCF77 module,
* I2C IMU, I2C ultrasonic distance sensors, GPS module and more
* sensors connected to free ADCs and IOs and to the I2C bus.
* This library doesn't contain functions for these sensors, because they are
* not HARDWIRED to the MultiIO Project Board and may be connected OPTIONALLY.
*
* ****************************************************************************
*/
/*****************************************************************************/
// MultiIO hardwired components:
// - I2C Voltage & Current Sensor (LTC2990)
// - I2C Real Time Clock (RTC DS1307Z)
// - I2C Temperature Sensor (TCN75A)
// - I2C Servo Controller (PCA9685)
// - I2C EEPROM (24LCXXX)
// - 3V3 Voltage Sensor
// - Touch Sensor (with NE555)
// - Buttons
// - LEDs
// - Buzzer
/*****************************************************************************/
// Includes:
#include "RP6M256_MultiIOLib.h"
/*****************************************************************************/
// Variables:
uint8_t registerBuf[13];
/*****************************************************************************/
// I2C Voltage & Current Sensor (LTC2990):
/**
* Sends the configuration byte to a LTC2990.
*
* Input: Config byte for control register
*
* There is also a macro LTC2990_init(), which
* initializes the sensor to work in V1-V2, V3,
* V4 and Single Acquisition mode.
*
* Example:
* LTC2990_init();
*
*/
void LTC2990_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_VCS_ADR, LTC2990_CONTROL, config);
}
uint8_t vcs_tint_low;
uint8_t vcs_tint_high;
uint8_t vcs_v1_low;
uint8_t vcs_v1_high;
uint8_t vcs_v2_low;
uint8_t vcs_v2_high;
uint8_t vcs_v3_low;
uint8_t vcs_v3_high;
uint8_t vcs_v4_low;
uint8_t vcs_v4_high;
uint8_t vcs_vcc_low;
uint8_t vcs_vcc_high;
/**
* Starts a Single Acquisition measurement.
*/
void LTC2990_run(void)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_VCS_ADR, LTC2990_TRIGGER, 0);
}
/**
* Reads all data registers of the voltage & current
* sensor (VCS).
* They are stored in the variables defined above.
*/
void LTC2990_read(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_VCS_ADR, LTC2990_TINT_MSB);
I2CTWI_readBytes(I2C_MULTIIO_VCS_ADR, registerBuf, 12);
vcs_tint_high = registerBuf[0];
vcs_tint_low = registerBuf[1];
vcs_v1_high = registerBuf[2];
vcs_v1_low = registerBuf[3];
vcs_v2_high = registerBuf[4];
vcs_v2_low = registerBuf[5];
vcs_v3_high = registerBuf[6];
vcs_v3_low = registerBuf[7];
vcs_v4_high = registerBuf[8];
vcs_v4_low = registerBuf[9];
vcs_vcc_high = registerBuf[10];
vcs_vcc_low = registerBuf[11];
}
double tint; // Internal temperature
double cbat; // Current at 9V
double vbat; // Battery voltage 9V (BAT)
double vservo; // Servo voltage 5 .. 7.5V
double vcc; // Voltage 5V (VCC)
/**
* Calculates the temperature, voltage and current
* values by using the data read from the LTC2990
* with the function LTC2990_read(). The sensor is
* configured to work in V1-V2, V3, V4 and Single
* Acquisition mode.
* The result is stored in the double variables
* defined above.
*/
void LTC2990_calculate(void)
{
int16_t tmp;
double v2;
tmp = (((vcs_tint_high & 0x1f) << 8) + vcs_tint_low) << 3;
tmp = tmp / 8;
tint = tmp / 16.0; // Internal temperature [°C]
tmp = (((vcs_v2_high & 0x7f) << 8) + vcs_v2_low) << 1;
tmp = tmp / 2;
v2 = tmp * 0.01942;
v2 *= C_ADJUST; // Current adjust factor
cbat = v2 / SHUNT_R + C_OFFSET; // Battery current [mA]
tmp = ((vcs_v3_high & 0x3f) << 8) + vcs_v3_low;
vbat = tmp * 0.00030518; // Battery voltage [V]
vbat *= VBAT_ADJUST; // Voltage divider factor
tmp = ((vcs_v4_high & 0x3f) << 8) + vcs_v4_low;
vservo = tmp * 0.00030518; // Servo voltage [V]
vservo *= VSERVO_ADJUST; // Voltage divider factor
tmp = ((vcs_vcc_high & 0x3f) << 8) + vcs_vcc_low;
vcc = 2.5 + tmp * 0.00030518; // VCC [V]
}
/**
* Performs a complete measurement with the sensor
* configured to work in V1-V2, V3, V4 and Single
* Acquisition mode.
* The result is stored in the double variables
* defined above.
* This function is BLOCKING for about 200 ms!
* If you don't want a blocking measurement, you
* have to write your own function with a non
* blocking pause between starting measurement
* and reading the result or with another mode
* of operation (Repeated Acquisitions).
*/
void LTC2990_measure(void)
{
LTC2990_run(); // Start measurement
mSleep(200);
LTC2990_read(); // Read data
LTC2990_calculate(); // Calculate values
}
/*****************************************************************************/
// I2C Real Time Clock (RTC DS1307Z):
/**
* This function converts a BCD to a DEC value.
*
*/
uint8_t BCD2DEC(uint8_t bcd)
{
return ((bcd >> 4) * 10 + (bcd & 0x0f));
}
/**
* This function converts a DEC to a BCD value.
*
*/
uint8_t DEC2BCD(uint8_t dec)
{uint8_t units = dec % 10;
if (dec /= 10) {
return (units + (DEC2BCD(dec) << 4));
}
else {
return units;
}
}
/**
* Sends the configuration byte to a DS1307.
*
* Input: Config byte for control register
*
*/
void DS1307_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_RTC_ADR, DS1307_CONTROL, config);
}
/**
* Initializes the DS1307 by resetting all registers.
* Only use this function ONCE for a DS1307 without or
* with empty backup battery!
*/
void DS1307_init(void)
{
uint8_t i;
for (i = 0; i < 8; i++) {
registerBuf[i] = 0;
}
I2CTWI_transmitBytes(I2C_MULTIIO_RTC_ADR, ®isterBuf[0], 8);
}
/**
* Reads all data registers of the Real Time Clock (RTC).
* They are stored in the time & date variables defined in
* the library header.
*/
void DS1307_read(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_RTC_ADR, DS1307_SECONDS);
I2CTWI_readBytes(I2C_MULTIIO_RTC_ADR, registerBuf, 7);
rtc_time.second = BCD2DEC(registerBuf[0] & 0x7f);
rtc_time.minute = BCD2DEC(registerBuf[1]);
rtc_time.hour = BCD2DEC(registerBuf[2] & 0x3f);
rtc_date.weekday = registerBuf[3];
rtc_date.day = BCD2DEC(registerBuf[4]);
rtc_date.month = BCD2DEC(registerBuf[5]);
rtc_date.year = BCD2DEC(registerBuf[6]) + 2000;
rtc_time.dst = 0;
#ifdef CALC_DST
// Calculate MESZ (DST):
uint8_t wday = rtc_date.weekday; // Weekday [1..7 = R_MO..R_SU]
if(wday == 7) wday = 0;
if(rtc_date.month < 3 || rtc_date.month > 10) {
return;
}
if((rtc_date.day - wday >= 25)
&& (wday || rtc_time.hour >= 2)) {
if(rtc_date.month == 10)
return;
}
else {
if(rtc_date.month == 3) {
return;
}
}
rtc_time.dst = 1;
#endif
}
/**
* Writes the time & date infos in the variables defined in
* the library header to the Real Time Clock (RTC).
*/
void DS1307_write(void)
{
registerBuf[0] = DS1307_SECONDS;
registerBuf[1] = DEC2BCD(rtc_time.second);
registerBuf[2] = DEC2BCD(rtc_time.minute);
registerBuf[3] = DEC2BCD(rtc_time.hour);
registerBuf[4] = rtc_date.weekday;
registerBuf[5] = DEC2BCD(rtc_date.day);
registerBuf[6] = DEC2BCD(rtc_date.month);
registerBuf[7] = DEC2BCD(rtc_date.year - 2000);
I2CTWI_transmitBytes(I2C_MULTIIO_RTC_ADR, ®isterBuf[0], 8);
}
/**
* Reads and returns a data byte from the DS1307 RAM.
*
* Input: adr -> RAM address [0..55]
*
* Hints: - The real DS1307 RAM addresses are 8..63!
* - The RAM is nonvolatile. That means, that
* it keeps the data as long as the backup
* battery doesn't become weak!
*
*/
uint8_t DS1307_readRAM(uint8_t adr)
{
if (adr > 55) adr = 0;
I2CTWI_transmitByte(I2C_MULTIIO_RTC_ADR, (DS1307_RAM + adr));
return (I2CTWI_readByte(I2C_MULTIIO_RTC_ADR));
}
/**
* Writes a data byte to the DS1307 RAM.
*
* Input: adr -> RAM address [0..55]
* data -> Data byte [0..255]
*
* Hints: - The real DS1307 RAM addresses are 8..63!
* - The RAM is nonvolatile. That means, that
* it keeps the data as long as the backup
* battery doesn't become weak!
*
*/
void DS1307_writeRAM(uint8_t adr, uint8_t data)
{
if (adr > 55) adr = 0;
I2CTWI_transmit2Bytes(I2C_MULTIIO_RTC_ADR, (DS1307_RAM + adr), data);
}
/*****************************************************************************/
// I2C Temperature Sensor (TCN75A):
/**
* Sends the configuration byte to a TCN75A.
*
* Input: Config byte for config register
*
*/
void TCN75_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_TEMP_ADR, TCN75_CONFIG, config);
}
uint8_t temperature_low;
uint8_t temperature_high;
/**
* Reads the two data registers of the temperature
* sensor.
* They are stored in the variables temperature_low
* and _high.
*
* Hint: Depending on the sensor configuration the
* data are located different in the two bytes
* - have a look at the datasheet!
*
*/
void TCN75_read(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_TEMP_ADR, TCN75_TEMP);
I2CTWI_readBytes(I2C_MULTIIO_TEMP_ADR, registerBuf, 2);
temperature_low = registerBuf[0];
temperature_high = registerBuf[1];
}
double temperature; // Temperature [°C]
/**
* Calculates and returns the temperature value
* by using the data read from the TCN75A with
* the function TCN75_read(). The sensor is
* configured to Single Conversion and 12 bit
* measurement.
*/
double TCN75_calculate(void)
{
uint8_t templow;
double temp;
templow = getTemperatureLow();
if (templow & 128) // Calculate temperature
templow = (templow & 63) - 127;
else
templow = templow & 63;
temp = templow + (0.0625 * (getTemperatureHigh() >> 4));
return temp;
}
/**
* Performs a 12 bit measurement and returns the
* temperature [°C].
* This function is BLOCKING for about 250 ms!
* If you don't want a blocking measurement, you
* have to write your own function with a non
* blocking pause between starting measurement
* and reading the result or with another mode
* of operation (Continuous Conversion).
*/
double TCN75_measure(void)
{
TCN75_run(TCN75A_CONFIG_RES_12); // Start measurement
mSleep(250);
TCN75_shutdown(); // Stop measurement
TCN75_read(); // Read data
return (TCN75_calculate()); // Calculate value
}
/*****************************************************************************/
// I2C Servo Controller (PCA9685):
/**
* Call this once before using the servo function.
*
* Input: PWM frequency [40..1000 Hz]
*
* Hints: - Default servo frequency is 50 Hz!
* - The servo power is NOT switched on by
* this function!
*
* There is also a macro initServo(freq), which
* does exactly the same as this function.
*
* Example:
* initServo(50);
*
*/
void PCA9685_init(uint16_t freq)
{
if ((freq < 40) || (freq > 1000)) freq = 50;
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE2);
uint8_t last_mode = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
last_mode &= ~PCA9685_MODE2_INVRT; // Clear INVRT bit
last_mode |= PCA9685_MODE2_OUTDRV; // Set OUTDRV bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE2, last_mode);
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1);
last_mode = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
last_mode |= PCA9685_MODE1_AI; // Set AI bit
uint8_t mode1 = last_mode;
mode1 |= PCA9685_MODE1_SLEEP; // Set SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
uint8_t prescale = (uint8_t) (F_PCA9685 / 4096 / freq - 0.5);
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_PRE_SCALE, prescale);
last_mode &= ~PCA9685_MODE1_SLEEP; // Clear SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, last_mode);
mSleep(1);
last_mode |= PCA9685_MODE1_RESTART; // Clear RESTART bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, last_mode);
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_LED8_ON_H, 0x10);
return;
}
/**
* This is the servo position set function.
*
* Input: servo -> Servo number [1..8, 10..16]
* pos -> Servo position [SERVOx_LT..SERVOx_RT]
*
* Hints: - Servo number 9 cannot be set by this function!
* Please use setServoPower() function instead!
* - A servo position of 205 means 1 ms servo impulse,
* a position of 410 means a 2 ms servo impulse!
* You may calculate the servo impulse length by:
* ==> Impulse [ms] = servo position / 204.8 <==
* (Formula only valid for a PWM of 50 Hz!)
*
* There is also a macro setServo(servo, pos), which
* does exactly the same as this function.
*
* Example:
* setServo(2,300);
*
*/
void PCA9685_set(uint8_t servo, uint16_t pos)
{
if ((servo == 0) || (servo == CHSERVOPWR) || (servo > 16))
return;
uint8_t reg = servo * 4 + 4; // Register LEDx_OFF_L
I2CTWI_transmit3Bytes(I2C_MULTIIO_SERVO_ADR, reg, (pos & 0x00ff), (pos >> 8));
}
/**
* If the servos are not moving for a while, the
* servo function can be stopped with this
* function (PCA9685 set to sleep mode).
*
* Hint: The servo power is NOT switched off by
* this function!
*
*/
void PCA9685_shutdown(void)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0x10);
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1);
uint8_t mode1 = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
mode1 |= PCA9685_MODE1_SLEEP; // Set SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
}
/**
* If the servo function was stopped with the
* function PCA9685_shutdown() before, it can be
* (re)started again with this function.
*
* Hint: The servo power is NOT switched on by
* this function!
*
*/
void PCA9685_restart(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1);
uint8_t mode1 = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
if (mode1 & PCA9685_MODE1_RESTART) { // RESTART bit set?
mode1 &= ~PCA9685_MODE1_SLEEP; // Clear SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
mSleep(1);
mode1 |= PCA9685_MODE1_RESTART; // Clear RESTART bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0);
}
}
/**
* With this function you can switch the servo
* power on or off, if the servo power jumper on
* the board enables this feature.
*
* Input: pwr -> 0 (false) = servo power off
* >0 (true) = servo power on
*
* Hints: - If connected servos are not used, you
* should always switch the servo power off
* to save energy!
* - The PCA9685 is NOT restarted or put into
* shutdown mode by this function!
*
*/
void setServoPower(uint8_t pwr)
{
if (pwr > 0)
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_LED8_OFF_H, 0);
else
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_LED8_OFF_H, 0x10);
}
/*****************************************************************************/
// I2C EEPROM (24LCXXX):
// The following EEPROM types may be used:
// EEPROM Size: Chip Pagesize
// - 32 kbit: ST24LC32A 32
// - 64 kbit: ST24LC64 32
// - 128 kbit: ST24LC128 64
// - 256 kbit: ST24LC256 64
// - 512 kbit: ST24LC512 128
// - 1024 kbit: AT24LC1024 256
// ! You may choose the EEPROM type in the I2C EEPROM !
// ! section of the RP6M256_MultiIO.h file! !
/**
* Reads a single Byte from the EEPROM.
*/
uint8_t I2C_EEPROM_readByte(uint16_t memAddr)
{
uint8_t data;
I2CTWI_transmit2Bytes(I2C_MULTIIO_EEPROM_ADR, (memAddr >> 8), memAddr);
data = I2CTWI_readByte(I2C_MULTIIO_EEPROM_ADR);
return data;
}
/**
* Write a single data byte to the specified EEPROM address.
*/
void I2C_EEPROM_writeByte(uint16_t memAddr, uint8_t data)
{
I2CTWI_transmit3Bytes(I2C_MULTIIO_EEPROM_ADR, (memAddr >> 8), memAddr, data);
mSleep(5);
}
/**
* Reads "length" Bytes into the Buffer "buffer" from startAddr on.
* You can read the complete EEPROM into a buffer at once - if it is large enough.
* (But you only have 8KB SRAM on a MEGA2560 ;) )
* If "length" is higher than I2CTWI_BUFFER_REC_SIZE defined in RP6I2CmasterTWI.h,
* you have to adapt I2CTWI_BUFFER_REC_SIZE to the highest used "length" value!
*/
void I2C_EEPROM_readBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_EEPROM_ADR, (startAddr >> 8), startAddr);
I2CTWI_readBytes(I2C_MULTIIO_EEPROM_ADR, &buffer[0], length);
}
/**
* Write "length" Bytes from the Buffer to the EEPROM.
* YOU CAN ONLY WRITE MAXIMAL [I2C_EEPROM_PAGESIZE] BYTES AT ONCE!!!
* This is the Pagesize!
* You can NOT cross a page boundary!
* If (length + 2) is higher than I2CTWI_BUFFER_SIZE defined in RP6I2CmasterTWI.h,
* you have to adapt I2CTWI_BUFFER_SIZE to the highest used (length + 2) value!
*/
void I2C_EEPROM_writeBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length)
{
uint8_t i, addrbuffer[length + 2];
addrbuffer[0] = (startAddr >> 8);
addrbuffer[1] = startAddr;
for(i = 0; i < length; i++) {
addrbuffer[i + 2] = buffer[i];
}
I2CTWI_transmitBytes(I2C_MULTIIO_EEPROM_ADR, &addrbuffer[0], (length + 2));
mSleep(5);
}
/*****************************************************************************/
// 3V3 Voltage Sensor:
uint16_t adc3v3; // 3V3 voltage sensor ADC value
/**
* This function reads and returns the ADC value of
* the 3V3 voltage sensor. The value is also stored
* in adc3v3.
*
*/
uint16_t get3V3Sensor(void)
{
adc3v3 = readADC(ADC_MULTIIO_3V3);
return adc3v3;
}
double v3v3; // 3V3 voltage [V]
/**
* Calculates and returns the 3.3V voltage value
* by using the data read from the 3V3 voltage
* sensor with the function get3V3Sensor().
*
*/
double calculate3V3(void)
{
return (5.0 / 1024.0 * adc3v3);
}
/**
* Measures and returns the 3.3V voltage value.
* The ADC value of the 3V3 voltage sensor is also
* stored in adc3v3.
*
*/
double measure3V3(void)
{
adc3v3 = readADC(ADC_MULTIIO_3V3);
return (5.0 / 1024.0 * adc3v3);
}
/*****************************************************************************/
// Touch Sensor (with NE555):
uint16_t adcTouch; // Touch sensor ADC value
uint8_t touch = 0; // True (1), if touched
/**
* Checks if the touch sensor antenna is touched - returns 1,
* if touched or 0, if the antenna is NOT touched.
*
*/
uint8_t getTouch(void)
{
adcTouch = readADC(ADC_MULTIIO_TOUCH);
if (adcTouch > ADCVAL_LIMIT_T) return 1;
else return 0;
}
/*****************************************************************************/
// Buttons:
uint16_t adcButtons; // Keypad ADC value
uint8_t releasedButtonNumber; // Released (last pressed) button
uint8_t pressedButtonNumber; // Actually pressed button
/**
* Checks which button is pressed - returns the button number,
* or 0, if no button is pressed.
* Maybe the values of ADCVAL_LIMITxx have to change because
* of variations in the resistors of the keypad! This is done
* in RP6M256_MultiIO.h, if you define other ADC values for
* your 4 buttons in ADCVAL_BUTTON1..ADCVAL_BUTTON4.
*
*/
uint8_t getPressedButtonNumber(void)
{
adcButtons = readADC(ADC_MULTIIO_BUTTONS);
if(adcButtons < 1020) {
nop();
nop();
nop();
adcButtons += readADC(ADC_MULTIIO_BUTTONS);
adcButtons >>= 1;
}
if(adcButtons < ADCVAL_LIMIT12)
return 1;
if(adcButtons < ADCVAL_LIMIT23)
return 2;
if(adcButtons < ADCVAL_LIMIT34)
return 3;
if(adcButtons < ADCVAL_LIMIT40)
return 4;
return 0;
}
/**
* This function has to be called frequently out of the
* main loop and checks if a button is pressed! It only returns
* the button number a single time, DIRECTLY when the button is
* pressed.
*
* This is useful for non-blocking keyboard check in the
* main loop. You don't need something like
* "while(getPressedButtonNumber());" to wait for the button
* to be released again!
*/
uint8_t checkPressedButtonEvent(void)
{
static uint8_t pressed_button = 0;
if(pressed_button) {
if(!getPressedButtonNumber())
pressed_button = 0;
}
else {
pressed_button = getPressedButtonNumber();
if(pressed_button)
return pressed_button;
}
return 0;
}
/**
* This function has to be called frequently out of
* the main loop and checks if a button is pressed AND
* released. It only returns the button number a single
* time, AFTER the button has been released.
*
* This is useful for non-blocking keyboard check in the
* main loop. You don't need something like
* "while(getPressedButtonNumber());" to wait for the button
* to be released again!
*/
uint8_t checkReleasedButtonEvent(void)
{
static uint8_t released_button = 0;
if(released_button) {
if(!getPressedButtonNumber()) {
uint8_t tmp = released_button;
released_button = 0;
return tmp;
}
}
else
released_button = getPressedButtonNumber();
return 0;
}
/*****************************************************************************/
// LEDs:
/**
* Set the 4 status LEDs of the MultiIO.
*
* Example:
* setMultiIOLEDs(0b1010);
* // this clears LEDs 1 and 3
* // and sets LEDs 2 and 4!
*
*/
void setMultiIOLEDs(uint8_t leds)
{
if (leds & 0b00000001) PCA9685_set(CHLED1, 4095);
else PCA9685_set(CHLED1, 0);
if (leds & 0b00000010) PCA9685_set(CHLED2, 4095);
else PCA9685_set(CHLED2, 0);
if (leds & 0b00000100) PCA9685_set(CHLED3, 4095);
else PCA9685_set(CHLED3, 0);
if (leds & 0b00001000) PCA9685_set(CHLED4, 4095);
else PCA9685_set(CHLED4, 0);
}
/**
* Dim the 4 status LEDs of the MultiIO.
*
* Input: led -> LED number [1..4]
* duty -> Duty cycle [0..4095]
*
* Example:
* dimMultiIOLED(2,2048);
* // this dims LED2 with a
* // duty cycle of 50% !
*
*/
void dimMultiIOLED(uint8_t led, uint16_t duty)
{
if (led == 1) PCA9685_set(CHLED1, duty);
if (led == 2) PCA9685_set(CHLED2, duty);
if (led == 3) PCA9685_set(CHLED3, duty);
if (led == 4) PCA9685_set(CHLED4, duty);
}
/**
* Set ONLY LED1, don't change anything for the other LEDs.
*
* Input: led -> 0 (false) = LED off
* >0 (true) = LED on
*
*/
void setMultiIOLED1(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED1, 4095);
else PCA9685_set(CHLED1, 0);
}
/**
* Set ONLY LED2, don't change anything for the other LEDs.
*/
void setMultiIOLED2(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED2, 4095);
else PCA9685_set(CHLED2, 0);
}
/**
* Set ONLY LED3, don't change anything for the other LEDs.
*/
void setMultiIOLED3(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED3, 4095);
else PCA9685_set(CHLED3, 0);
}
/**
* Set ONLY LED4, don't change anything for the other LEDs.
*/
void setMultiIOLED4(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED4, 4095);
else PCA9685_set(CHLED4, 0);
}
/*****************************************************************************/
// Buzzer:
/**
* Call this once before using the buzzer.
*
*/
void Buzzer_init(void)
{
DDRB |= OC2A_PI4; // Portpin: output
TCCR2A = 0;
TCCR2B = 0; // Sound off
PORTB &= ~OC2A_PI4; // Portpin low
}
/**
* This function has no timing stuff. It only sets the pitch
* and this can be used to generate tone sequences which
* would sound bad if the beeper turns of for a very short time
* in between - such as alarm tones or special melodies etc.
*
* Input: pitch -> 0 = sound off
* 1 = lowest frequency
* 255 = higest frequency
*
*/
void setBuzzerPitch(uint8_t pitch)
{
OCR2A = 255 - pitch;
if(pitch) {
TCCR2A = (1 << WGM21) | (1 << COM2A0);
TCCR2B = (1 << CS21) | (1 << CS22);
}
else {
TCCR2A = 0;
TCCR2B = 0; // Normal port operation
PORTB &= ~OC2A_PI4; // Portpin low
}
}
/**
* You can use this function to make the buzzer beep ;)
* This function is BLOCKING and generates a delay for the
* sound and a delay between two sounds.
*
* Input: pitch -> 0 = sound off
* 1 = lowest frequency
* 255 = higest frequency
* time -> sound length [ms]
* delay -> pause after the sound [ms]
*
* Example:
* soundBuzzer(150,50,25);
* soundBuzzer(200,50,25);
*
* There is also a macro buzzer(pitch, time). You may use
* it for sounds without a pause after the sound. The macro
* is BLOCKING during the sound length.
*
* Example:
* buzzer(150,50);
*
*/
void soundBuzzer(uint8_t pitch, uint16_t time, uint16_t delay)
{
setBuzzerPitch(pitch);
if (!pitch) return;
mSleep(time);
setBuzzerPitch(0); // Sound off
if (!delay) return;
mSleep(delay);
}
/*****************************************************************************/
/*****************************************************************************/
// MultiIO Project Board initialisation and shutdown:
/**
* You MUST call this function at the beginning of a
* main program, that uses the MultiIO Project Board.
*
*/
void multiio_init(void)
{
// Voltage & current sensor:
LTC2990_init(); // Init VCS
// Servo Controller:
PCA9685_init(50); // Init PWM 50 Hz
// Servo power:
setServoPower(0); // Servo power off
// Buzzer:
Buzzer_init(); // Init buzzer
}
/**
* If you don't use any function of the MultiIO
* Project Board, you can put the MultiIO into
* "SHUTDOWN MODE". In this mode the electric
* power consumption is very low to save energy.
*
*/
void multiio_shutdown(void)
{
// LEDs:
setMultiIOLEDs(0b0000); // MultiIO LEDs off
// Servo power:
setServoPower(0); // Servo power off
// Servo Controller:
PCA9685_shutdown(); // Shutdown PCA9685
// Temperature Sensor:
TCN75_shutdown(); // Shutdown TCN75A
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 2.2 (shutdown mode added) 28.03.2013 by Dirk
* - v. 2.1 (LTC2990 current bug fixed) 26.03.2013 by Dirk
* - v. 2.0 (release for board V1.0) 17.02.2013 by Dirk
* - v. 1.0 (initial release) 21.01.2013 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6M256_MultiIO_01 ... SRC += RP6M256_MultiIOLib.c ...
Datei RP6M256_MultiIO_01.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a first test for the MultiIO Project Board.
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 MultiIO library":
// (This is the library for accessing the MultiIO Project Board!)
#include "RP6M256_MultiIOLib.h"
/*****************************************************************************/
/**
* Write a floating point number to the WIFI.
*
* Example:
*
* // Write a floating point number to the WIFI (no exponent):
* writeDouble_WIFI(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString_WIFI(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 1!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", " Selftest 1");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
uint8_t onoff = 0;
uint16_t servopos = SERVO1_LT;
startStopwatch1();
// IMPORTANT:
multiio_init(); // MultiIO init!!!
//setServoPower(1); // Servo power ON!
// ----------------------------------------------
// Set RTC once (battery empty or not existing:
rtc_time.second = 0;
rtc_time.minute = 0;
rtc_time.hour = 12; // 12:00
rtc_date.weekday = R_TH;
rtc_date.day = 28;
rtc_date.month = 3;
rtc_date.year = 2013; // Do, 28.3.2013
DS1307_write();
// Remove this, if RTC is set and running!!!
// ----------------------------------------------
// EEPROM test:
writeString_P_WIFI("\nWriting 128 to EEPROM address 5:\n");
I2C_EEPROM_writeByte(5, 128);
mSleep(500);
writeString_P_WIFI("Done!\n");
writeString_P_WIFI("\nReading EEPROM address 5:\n");
uint8_t tmp = I2C_EEPROM_readByte(5);
mSleep(500);
I2C_EEPROM_writeByte(5, 0);
writeString_P_WIFI("Done!\n");
writeString_P_WIFI("EEPROM address 5 content: ");
writeInteger_WIFI(tmp, DEC);
writeString_P_WIFI("\n");
// Buzzer test:
soundBuzzer(Tone_Cis2, 300, 200);
soundBuzzer(Tone_Fis2, 200, 100);
soundBuzzer(Tone_Ais2, 100, 100);
soundBuzzer(Tone_Dis3, 50, 100);
mSleep(1000);
soundBuzzer(Tone_Dis3, 300, 200);
soundBuzzer(Tone_Ais2, 200, 100);
soundBuzzer(Tone_Fis2, 100, 100);
soundBuzzer(Tone_Cis2, 50, 100);
while(true)
{
if(getStopwatch1() > 1000) // 1s
{
if (onoff) onoff = 0;
else onoff = 1;
// Buttons ADC test:
clearLCD();
pressedButtonNumber = getPressedButtonNumber();
setCursorPosLCD(0, 0);
writeStringLCD("Button: ");
writeIntegerLCD(pressedButtonNumber, DEC);
setCursorPosLCD(1, 0);
writeStringLCD("ADC: ");
writeIntegerLCD(adcButtons, DEC);
// 3V3 voltage sensor ADC test:
v3v3 = measure3V3();
writeString_WIFI("\n3V3 Voltage: ");
writeDouble_WIFI(v3v3, 4, 1);
writeString_WIFI("V\nADC 3V3: ");
writeInteger_WIFI(adc3v3, DEC);
// Touch sensor ADC test:
touch = getTouch();
if (touch) writeString_WIFI("\nTOUCHED!!!");
else writeString_WIFI("\nNOT touched.");
writeString_WIFI("\nADC Touch: ");
writeInteger_WIFI(adcTouch, DEC);
// Temperature sensor test:
temperature = TCN75_measure(); // Measure
writeString_WIFI("\nTemperature: ");
writeDouble_WIFI(temperature, 5, 1);
writeString_WIFI("°\n");
// Servo controller test:
// LEDs:
if (onoff) {
setMultiIOLED1(1);
setMultiIOLED2(0);
setMultiIOLED3(0);
setMultiIOLED4(1);
}
else
setMultiIOLEDs(0b0110);
// Servo 1:
setServo(1, servopos);
servopos += 10;
if (servopos > SERVO1_RT) servopos = SERVO1_LT;
// RTC test:
DS1307_read();
writeString_WIFI("RTC: ");
writeIntegerLength_WIFI(rtc_time.hour, DEC, 2);
writeString_WIFI(":");
writeIntegerLength_WIFI(rtc_time.minute, DEC, 2);
writeString_WIFI(":");
writeIntegerLength_WIFI(rtc_time.second, DEC, 2);
writeString_WIFI(" ");
writeIntegerLength_WIFI(rtc_date.day, DEC, 2);
writeString_WIFI(".");
writeIntegerLength_WIFI(rtc_date.month, DEC, 2);
writeString_WIFI(".");
writeIntegerLength_WIFI(rtc_date.year, DEC, 4);
writeString_WIFI("\n");
// Voltage & current sensor test:
LTC2990_measure();
writeString_WIFI("Temperature: ");
writeDouble_WIFI(tint, 5, 1);
writeString_WIFI("°\n");
writeString_WIFI("BAT Current: ");
writeDouble_WIFI(cbat, 6, 1);
writeString_WIFI("mA\nBAT Voltage: ");
writeDouble_WIFI(vbat, 4, 1);
writeString_WIFI( "V\nSERVO Volt.: ");
writeDouble_WIFI(vservo, 4, 1);
writeString_WIFI( "V\nVCC Voltage: ");
writeDouble_WIFI(vcc, 4, 1);
writeString_WIFI("V\n");
// MultiIO shutdown:
if (pressedButtonNumber == 4) {
writeString_WIFI("\nPress button 1 for MultiIO SHUTDOWN");
writeString_WIFI("\nor any other button to continue!!!\n");
do {
mSleep(1);
releasedButtonNumber = checkReleasedButtonEvent();
task_I2CTWI();
} while (!releasedButtonNumber);
if (releasedButtonNumber == 1) {
writeString_WIFI("\nPlease wait for MultiIO SHUTDOWN...\n");
multiio_shutdown();
mSleep(3000);
writeString_WIFI("\nThe MultiIO now is in SHUTDOWN MODE!!!\n");
mSleep(1000);
writeString_WIFI("\nRESET the M256 microcontroller now...\n\n");
while(true) {};
}
}
setStopwatch1(0);
}
task_I2CTWI();
}
return 0;
}
Erklärung
Linienfolger und Bumper Board Library
Diese Avr-gcc Library für das Linienfolger und das Bumper Board des Multi IO Projekts geht von folgenden Voraussetzungen aus:
- Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt.
- Die M256 ist der I2C-Bus Master.
- Die I2C-Bus Geschwindigkeit beträgt 100 kHz.
- Alle Jumper auf der MultiIO sind in ihrer Standardstellung (1).
- Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden.
- Der Wannenstecker IO_Mxxx der MultiIO ist mit dem Wannenstecker IO_PWM/T2/T3 der M256 1:1 verbunden.
- Der Wannenstecker ADC_Mxxx der MultiIO ist mit dem Wannenstecker ADC_IO2/CMP der M256 1:1 verbunden.
- Der Wannenstecker ADC_M256 der MultiIO ist mit dem Wannenstecker ADC_IO1 der M256 1:1 verbunden.
- Das Linienfolger und/oder das Bumper Board ist an den Stecker "CNY70" (P_CNY) bzw. "Bumper" (P_BUMPER) der MultiIO angeschlossen.
Zu (1): Siehe hier!
Linienfolger Sensoren (LFS) anschliessen
Will man die Linienfolger Sensoren (LFS) anstelle der 3,3V-Messung und des Touch Sensors nutzen (mit dieser Library für die M256), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block:
Hinweis: Die Jumper sind orange eingezeichnet!
I2C Sensoren (z.B. SRF02) anschliessen
Will man I2C Sensoren (z.B. SRF02) anstelle von analogen Sensoren (SHARP, Radar) auf dem Bumper Board nutzen (mit dieser Library für die M256), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block:
Hinweis: Die Jumper sind orange eingezeichnet!
Die Library (Software-Bibliothek) besteht aus zwei Teilen:
- Dem Library Header -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library.
- Der Library Source -> Das ist die eigentliche Library.
Zusätzlich wird auch der MultiIO Configuration Header eingebunden. Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss.
Library Header
Vor dem Kompilieren der Library muss eine Anpassung des Headers an die eigenen Hardware-Voraussetzungen erfolgen. Dazu gibt es im Header fünf Definitionen:
- LFS -> Linienfolger (= "LFS") Sensoren werden benutzt
- FIVE_SENSORS -> Es werden alle 5 LFS Sensoren (CNY70) benutzt (sonst nur 3!)
- BUMPERS -> Die Bumper werden verwendet
- SHARP_SENSORS -> Es werden analoge SHARP Sensoren benutzt ODER
- SRF02_SENSORS -> Es werden SRF02 Ultraschall Sensoren (I2C) benutzt
Im nachfolgenden Listing sind ALLE Definitionen aktiv. Dies muss an die eigene Hardware angepaßt werden. Wenn z.B. SRF02 NICHT benutzt werden, muss die Zeile mit "SRF02_SENSORS" auskommentiert werden. Dazu setzt man "//" an den Zeilenanfang. Beispiel:
//#define SRF02_SENSORS // SRF02 ultrasonic sensors are used
Datei: RP6M256_LFSBumperLib.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples * **************************************************************************** * Example: RP6M256 LFS Bumper Library * Author(s): Dirk * **************************************************************************** * Description: * Header file for new LFS and Bumper Board library. * * **************************************************************************** */ #ifndef RP6M256_LFSBUMPERLIB_H #define RP6M256_LFSBUMPERLIB_H /*****************************************************************************/ // LFS Board components: // - 3 or 5 reflective optical sensors (CNY70) // Bumper Board components: // - 2 LEDs // - 2 bumpers // - 2 SHARP IR distance sensors (e.g. GP2Y0A02YK) // - 2 SRF02 ultrasonic distance sensors (SRF02) // - 2 Radar sensors (RSM-1650 based) /*****************************************************************************/ // Includes: #include "RP6M256Lib.h" #include "RP6I2CmasterTWI.h" #include "RP6M256_MultiIO.h" /*****************************************************************************/ // Line Following Sensor (LFS) Board: // Define the use of the LFS Board and the number of CNY70s (3 or 5) to be // used here! #define LFS // LFS Board is used #define FIVE_SENSORS // Only, if all 5 CNY70s are used! // --------------------------------------------------------------------------- #define CH_LFS_L 1 #define CH_LFS_M 2 #define CH_LFS_R 3 #define CH_LFS_R1 4 #define CH_LFS_L1 5 #define ADCVAL_BLACKLINE 424 #define ADCVAL_WHITEPAPER 994 #define ADCVAL_LIMIT_BW ((ADCVAL_WHITEPAPER - ADCVAL_BLACKLINE) / 2 + ADCVAL_BLACKLINE) #ifdef LFS void LFS_init(void); uint16_t getLFS(uint8_t channel); void setLFSPower(uint8_t pwr); #endif /*****************************************************************************/ // Bumper Board: // Define the devices that are built up on the Bumper Board here! // Make a comment of a line belonging to a sensor that is NOT built // up on or attached to the Bumper Board! // IMPORTANT: It is only possible to use EITHER SHARP sensors OR SRF02 // sensors OR Radar sensors on the Bumper Board!!! // But in addition you may connect up to four SRF02 sensors // to the MultiIO Board! #define BUMPERS // Bumpers are used #define SHARP_SENSORS // SHARP IR distance sensors are used #define SRF02_SENSORS // SRF02 ultrasonic sensors are used #define RADAR_SENSORS // RSM-1650 Radar sensors are used // --------------------------------------------------------------------------- #ifndef LEFT #define LEFT 2 #endif #ifndef RIGHT #define RIGHT 3 #endif // SRF02 sensors (2) on the Bumper Board: #define I2C_MULTIIO_SRF02_L_ADR 0xe0 // Default #define I2C_MULTIIO_SRF02_R_ADR 0xe2 // SRF02 sensors (4) connected to the MultiIO Board: #define I2C_MULTIIO_SRF02_1_ADR 0xe4 #define I2C_MULTIIO_SRF02_2_ADR 0xe6 #define I2C_MULTIIO_SRF02_3_ADR 0xe8 #define I2C_MULTIIO_SRF02_4_ADR 0xea // Other possible SRF02 I2C addresses: //#define I2C_MULTIIO_SRF02_x_ADR 0xec //#define I2C_MULTIIO_SRF02_x_ADR 0xee //#define I2C_MULTIIO_SRF02_x_ADR 0xf0 //#define I2C_MULTIIO_SRF02_x_ADR 0xf2 //#define I2C_MULTIIO_SRF02_x_ADR 0xf4 //#define I2C_MULTIIO_SRF02_x_ADR 0xf6 //#define I2C_MULTIIO_SRF02_x_ADR 0xf8 //#define I2C_MULTIIO_SRF02_x_ADR 0xfa //#define I2C_MULTIIO_SRF02_x_ADR 0xfc //#define I2C_MULTIIO_SRF02_x_ADR 0xfe // SRF02 registers: #define SRF02_COMMAND 0 // Read: Software Revision #define SRF02_RANGE_MSB 2 #define SRF02_RANGE_LSB 3 #define SRF02_AUTOTUNE_MIN_MSB 4 #define SRF02_AUTOTUNE_MIN_LSB 5 // SRF02 mode constants: #define MODE_INCH 80 // Result in [inch] #define MODE_CM 81 // Result in [cm] #define MODE_US 82 // Result in [us] #define MODE_SYNC_INCH 86 // Result in [inch] * #define MODE_SYNC_CM 87 // Result in [cm] * #define MODE_SYNC_US 88 // Result in [us] * #define MODE_40KHZ_BURST 92 // Ping. No measurement! // *) Fake ranging mode! // Sonic speed constants in air (default: at 20°C): //#define SONIC_SPEED 325.4 // [m/s] at -10°C //#define SONIC_SPEED 328.5 // [m/s] at - 5°C //#define SONIC_SPEED 331.5 // [m/s] at 0°C //#define SONIC_SPEED 334.5 // [m/s] at 5°C //#define SONIC_SPEED 337.5 // [m/s] at 10°C //#define SONIC_SPEED 340.5 // [m/s] at 15°C #define SONIC_SPEED 343.4 // [m/s] at 20°C //#define SONIC_SPEED 346.3 // [m/s] at 25°C //#define SONIC_SPEED 349.2 // [m/s] at 30°C // SRF02 channels: #define CH_SRF02_L 1 #define CH_SRF02_R 2 #define CH_SRF02_1 3 #define CH_SRF02_2 4 #define CH_SRF02_3 5 #define CH_SRF02_4 6 // LEDs (always used): void LEDS_init(void); void setbumperLEDL(uint8_t led); void setbumperLEDR(uint8_t led); void setbumperLEDs(uint8_t leds); // Bumpers: #ifdef BUMPERS void MULTIIO_BUMPERS_init(void); uint8_t getMULTIIO_BUMPER(uint8_t side); extern uint8_t iobumper_l; extern uint8_t iobumper_r; void MULTIIO_BUMPERS_setStateChangedHandler(void (*bumperHandler)(void)); void task_MULTIIO_BUMPERS(void); #endif // SHARP sensors: #ifdef SHARP_SENSORS void SHARPS_init(void); extern uint16_t adcsharp_l; extern uint16_t adcsharp_r; uint16_t getSHARPSensor(uint8_t side); extern double distsharp_l; extern double distsharp_r; double calculateSHARP(uint8_t side); double measureSHARP(uint8_t side); void setSHARPSPower(uint8_t pwr); #endif // SRF02 sensors: #ifdef SRF02_SENSORS extern uint16_t distsrf02; uint8_t SRF02_i2cadr(uint8_t channel); uint8_t SRF02_getFirmware(uint8_t channel); void SRF02_ping(uint8_t channel, uint8_t mode); uint16_t SRF02_read(uint8_t channel); extern double distsrf02_l; extern double distsrf02_r; double SRF02_calculate(uint16_t us); uint16_t SRF02_measure(uint8_t channel, uint8_t mode); void SRF02_changeAdr(uint8_t adr, uint8_t newadr); #endif // Radar sensors: #ifdef RADAR_SENSORS void RADARS_init(void); extern uint16_t adcradar_l; extern uint16_t adcradar_r; uint16_t getRADARSensor(uint8_t side); extern double procradar_l; extern double procradar_r; double calculateRADAR(uint8_t side); double measureRADAR(uint8_t side); void setRADARSPower(uint8_t pwr); #endif /*****************************************************************************/ // LFS and Bumper Board initialisation and shutdown: void lfsbumper_init(void); void lfsbumper_shutdown(void); /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_LFSBumperLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Source
Datei: RP6M256_LFSBumperLib.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 LFS Bumper Library
* Author(s): Dirk
* ****************************************************************************
* Description:
*
* This is our new Library that contains basic routines and functions for
* accessing the components of the LFS and Bumper Board.
*
* ****************************************************************************
*/
/*****************************************************************************/
// LFS Board components:
// - 3 or 5 reflective optical sensors (CNY70)
// Bumper Board components:
// - 2 LEDs
// - 2 bumpers
// - 2 SHARP IR distance sensors (e.g. GP2Y0A02YK)
// - 2 SRF02 ultrasonic distance sensors (SRF02)
// - 2 Radar sensors (RSM-1650 based)
/*****************************************************************************/
// Includes:
#include "RP6M256_LFSBumperLib.h"
/*****************************************************************************/
// Variables:
uint8_t registerBuf[13];
/*****************************************************************************/
// Line Following Sensor (LFS) Board:
#ifdef LFS
/**
* Call this once before using the LFS function.
*
*/
void LFS_init(void)
{
IO_MULTIIO_LFS_PWR_DDR |= IO_MULTIIO_LFS_PWR_IN;
// LFS power on:
IO_MULTIIO_LFS_PWR_PORT &= ~IO_MULTIIO_LFS_PWR_IN;
}
/**
* This function reads the ADC values of the 5
* CNY70 sensors on the LFS board.
*
* Input: channel -> 1 = left LFS sensor (CH_LFS_L)
* 2 = middle LFS sensor (CH_LFS_M)
* 3 = right LFS sensor (CH_LFS_R)
* 4 = outer right LFS sensor (CH_LFS_R1)
* 5 = outer left LFS sensor (CH_LFS_L1)
*
*/
uint16_t getLFS(uint8_t channel)
{
uint16_t value;
switch (channel) {
case CH_LFS_L :
value = readADC(ADC_MULTIIO_LFS_L); break;
case CH_LFS_M :
value = readADC(ADC_MULTIIO_LFS_M); break;
case CH_LFS_R :
value = readADC(ADC_MULTIIO_LFS_R); break;
#ifdef FIVE_SENSORS
case CH_LFS_R1 :
value = readADC(ADC_MULTIIO_LFS_R1); break;
case CH_LFS_L1 :
value = readADC(ADC_MULTIIO_LFS_L1); break;
#endif
default : value = 0;
}
return value;
}
/**
* With this function you can switch the LFS
* power on or off.
*
* Input: pwr -> 0 (false) = LFS power off
* >0 (true) = LFS power on
*
* Hint: If the LFS is not used, you should
* always switch the LFS power off to
* save energy!
*
*/
void setLFSPower(uint8_t pwr)
{
if (pwr > 0)
IO_MULTIIO_LFS_PWR_PORT &= ~IO_MULTIIO_LFS_PWR_IN;
else
IO_MULTIIO_LFS_PWR_PORT |= IO_MULTIIO_LFS_PWR_IN;
}
#endif
/*****************************************************************************/
// Bumper Board:
// LEDs (always used):
union {
uint8_t byte;
struct {
unsigned LEDL:1;
unsigned LEDR:1;
unsigned unused:6;
};
} bumperLEDs; // Status of the bumper LEDs
/**
* Call this once before using the LEDs.
*
*/
void LEDS_init(void)
{
IO_MULTIIO_BUMPER_R_DDR |= IO_MULTIIO_BUMPER_R_IN;
IO_MULTIIO_BUMPER_R_PORT &= ~IO_MULTIIO_BUMPER_R_IN;
IO_MULTIIO_BUMPER_L_DDR |= IO_MULTIIO_BUMPER_L_IN;
IO_MULTIIO_BUMPER_L_PORT &= ~IO_MULTIIO_BUMPER_L_IN;
bumperLEDs.byte = 0;
}
/**
* Set ONLY LEDL, don't change anything for the
* right bumper LED.
*
* Input: led -> 0 (false) = LEDL off
* >0 (true) = LEDL on
*
*/
void setbumperLEDL(uint8_t led)
{
if (led > 0) {
IO_MULTIIO_BUMPER_L_PORT |= IO_MULTIIO_BUMPER_L_IN;
bumperLEDs.LEDL = 1;
}
else {
IO_MULTIIO_BUMPER_L_PORT &= ~IO_MULTIIO_BUMPER_L_IN;
bumperLEDs.LEDL = 0;
}
}
/**
* Set ONLY LEDR, don't change anything for the
* left bumper LED.
*
* Input: led -> 0 (false) = LEDR off
* >0 (true) = LEDR on
*
*/
void setbumperLEDR(uint8_t led)
{
if (led > 0) {
IO_MULTIIO_BUMPER_R_PORT |= IO_MULTIIO_BUMPER_R_IN;
bumperLEDs.LEDR = 1;
}
else {
IO_MULTIIO_BUMPER_R_PORT &= ~IO_MULTIIO_BUMPER_R_IN;
bumperLEDs.LEDR = 0;
}
}
/**
* Set the 2 LEDs of the MultiIO Bumper Board.
*
* Example:
* setbumperLEDs(0b10);
* // this clears LEDL
* // and sets LEDR!
*
*/
void setbumperLEDs(uint8_t leds)
{
if (leds & 0b00000001) {
setbumperLEDL(1);
}
else {
setbumperLEDL(0);
}
if (leds & 0b00000010) {
setbumperLEDR(1);
}
else {
setbumperLEDR(0);
}
}
// Bumpers:
#ifdef BUMPERS
/**
* Call this once before using the bumpers.
*
* ATTENTION: Stopwatch 8 is used for the bumpers
* task! Please do not use this
* stopwatch elsewhere in your program!
*
*/
void MULTIIO_BUMPERS_init(void)
{
startStopwatch8();
}
/**
* This function reads and returns the value of
* the left or right bumper.
*
* Input: side -> 2 (LEFT) = Left bumper
* 3 (RIGHT) = Right bumper
*
*/
uint8_t getMULTIIO_BUMPER(uint8_t side)
{
uint8_t tmp;
if (side == LEFT) {
IO_MULTIIO_BUMPER_L_PORT &= ~IO_MULTIIO_BUMPER_L_IN;
IO_MULTIIO_BUMPER_L_DDR &= ~IO_MULTIIO_BUMPER_L_IN;
sleep(5);
if ((IO_MULTIIO_BUMPER_L_PIN & IO_MULTIIO_BUMPER_L_IN) > 0)
tmp = 1;
else tmp = 0;
IO_MULTIIO_BUMPER_L_DDR |= IO_MULTIIO_BUMPER_L_IN;
if (bumperLEDs.LEDL) {
IO_MULTIIO_BUMPER_L_PORT |= IO_MULTIIO_BUMPER_L_IN;
}
return tmp;
}
if (side == RIGHT) {
IO_MULTIIO_BUMPER_R_PORT &= ~IO_MULTIIO_BUMPER_R_IN;
IO_MULTIIO_BUMPER_R_DDR &= ~IO_MULTIIO_BUMPER_R_IN;
sleep(5);
if ((IO_MULTIIO_BUMPER_R_PIN & IO_MULTIIO_BUMPER_R_IN) > 0)
tmp = 1;
else tmp = 0;
IO_MULTIIO_BUMPER_R_DDR |= IO_MULTIIO_BUMPER_R_IN;
if (bumperLEDs.LEDR) {
IO_MULTIIO_BUMPER_R_PORT |= IO_MULTIIO_BUMPER_R_IN;
}
return tmp;
}
return 0;
}
// -------------------------------
// MultiIO Bumpers State changed handler:
void MULTIIO_BUMPERS_stateChanged_DUMMY(void){}
static void (*MULTIIO_BUMPERS_stateChangedHandler)(void) = MULTIIO_BUMPERS_stateChanged_DUMMY;
/**
* Use this function to set the MultiIO Bumpers state change handler.
*
*/
void MULTIIO_BUMPERS_setStateChangedHandler(void (*bumperHandler)(void))
{
MULTIIO_BUMPERS_stateChangedHandler = bumperHandler;
}
// -------------------------------
uint8_t iobumper_l;
uint8_t iobumper_r;
/**
* If you call this frequently out of the mainloop,
* the global iobumper_l and iobumper_r variables
* are updated automatically every 50ms and can be
* used everywhere in your program. It can also
* call an event handler routine, that you need to
* register with MULTIIO_BUMPERS_setStateChangedHandler
* before.
*
*/
void task_MULTIIO_BUMPERS(void)
{
if(getStopwatch8() > 50) { // 50ms
uint8_t left = getMULTIIO_BUMPER(LEFT);
uint8_t right = getMULTIIO_BUMPER(RIGHT);
if(iobumper_l != left || iobumper_r != right) {
iobumper_l = left;
iobumper_r = right;
MULTIIO_BUMPERS_stateChangedHandler();
}
setStopwatch8(0);
}
}
#endif
// SHARP sensors:
#ifdef SHARP_SENSORS
/**
* Call this once before using the SHARPS.
*
*/
void SHARPS_init(void)
{
IO_MULTIIO_SHARPS_PWR_DDR |= IO_MULTIIO_SHARPS_PWR_IN;
// SHARPS power on:
IO_MULTIIO_SHARPS_PWR_PORT &= ~IO_MULTIIO_SHARPS_PWR_IN;
}
uint16_t adcsharp_l; // Left SHARP sensor ADC value
uint16_t adcsharp_r; // Right SHARP sensor ADC value
/**
* This function reads and returns the ADC value
* of the left or right SHARP sensor. The value is
* also stored in adcsharp_l or adcsharp_r.
*
* Input: side -> 2 (LEFT) = Left SHARP sensor
* 3 (RIGHT) = Right SHARP sensor
*
*/
uint16_t getSHARPSensor(uint8_t side)
{
if (side == LEFT) {
adcsharp_l = readADC(ADC_MULTIIO_SHARP_L);
return adcsharp_l;
}
if (side == RIGHT) {
adcsharp_r = readADC(ADC_MULTIIO_SHARP_R);
return adcsharp_r;
}
return 0;
}
double distsharp_l; // Left SHARP sensor distance
double distsharp_r; // Right SHARP sensor distance
/**
* Calculates and returns the left or right SHARP
* sensor distance value [mm] by using the data
* read from the SHARP sensor with the function
* getSHARPSensor().
*
* Input: side -> 2 (LEFT) = Left SHARP sensor
* 3 (RIGHT) = Right SHARP sensor
*
* Hints: - The linearisation formula is adapted to
* the SHARP GP2Y0A02YK sensor ONLY!
* - You can calculate your own linearisation
* formula by using a table calculation
* program!
*
*/
double calculateSHARP(uint8_t side)
{
double vside;
if (side == LEFT) {
vside = adcsharp_l * 5.0 / 1024.0;
vside = (0.082712905 + 9395.7652 * vside)
/ (1 - 3.3978697 * vside + 17.339222 * vside * vside);
return vside;
}
if (side == RIGHT) {
vside = adcsharp_r * 5.0 / 1024.0;
vside = (0.082712905 + 9395.7652 * vside)
/ (1 - 3.3978697 * vside + 17.339222 * vside * vside);
return vside;
}
return 0;
}
/**
* Measures and returns the left or right SHARP
* sensor distance value [cm].
* The ADC value of the SHARP sensor is also
* stored in adcsharp_l or adcsharp_r.
*
* Input: side -> 2 (LEFT) = Left SHARP sensor
* 3 (RIGHT) = Right SHARP sensor
*
*/
double measureSHARP(uint8_t side)
{
uint16_t tmp;
tmp = getSHARPSensor(side);
return (calculateSHARP(side) / 10); // Result in [cm]
}
/**
* With this function you can switch the SHARP
* sensors power on or off.
*
* Input: pwr -> 0 (false) = SHARPS power off
* >0 (true) = SHARPS power on
*
* Hint: If the SHARPS are not used, you should
* always switch the SHARPS power off to
* save energy!
*
*/
void setSHARPSPower(uint8_t pwr)
{
if (pwr > 0)
IO_MULTIIO_SHARPS_PWR_PORT &= ~IO_MULTIIO_SHARPS_PWR_IN;
else
IO_MULTIIO_SHARPS_PWR_PORT |= IO_MULTIIO_SHARPS_PWR_IN;
}
#endif
// SRF02 sensors:
#ifdef SRF02_SENSORS
uint16_t distsrf02; // SRF02 distance [inch, cm, us]
/**
* Returns the I2C slave address belonging to the
* SRF02 sensor with the channel number [1..6].
*
* Input: channel -> 1 = left SRF02 sensor (CH_SRF02_L) *
* 2 = right SRF02 sensor (CH_SRF02_R) *
* 3 = SRF02 sensor 1 (CH_SRF02_1) "
* 4 = SRF02 sensor 2 (CH_SRF02_2) "
* 5 = SRF02 sensor 3 (CH_SRF02_3) "
* 6 = SRF02 sensor 4 (CH_SRF02_4) "
* *) Located on the Bumper Board!
* ") Connected to the MultiIO Board!
*
*/
uint8_t SRF02_i2cadr(uint8_t channel)
{
uint8_t adr;
switch (channel) {
case CH_SRF02_L :
adr = I2C_MULTIIO_SRF02_L_ADR; break;
case CH_SRF02_R :
adr = I2C_MULTIIO_SRF02_R_ADR; break;
case CH_SRF02_1 :
adr = I2C_MULTIIO_SRF02_1_ADR; break;
case CH_SRF02_2 :
adr = I2C_MULTIIO_SRF02_2_ADR; break;
case CH_SRF02_3 :
adr = I2C_MULTIIO_SRF02_3_ADR; break;
case CH_SRF02_4 :
adr = I2C_MULTIIO_SRF02_4_ADR; break;
default : adr = I2C_MULTIIO_SRF02_L_ADR;
}
return adr;
}
/**
* Reads and returns the SRF02 firmware (software)
* revision number.
* If this function returns 255 after a "ping" was
* sent to the sensor (with SRF02_ping()!), the
* result of the measurement cannot be read yet
* over the I2C bus.
*
* Input: channel -> [1..6] (for details see
* function SRF02_i2cadr()!)
*
* Example:
* tmp = SRF02_getFirmware(CH_SRF02_L);
* // this reads the software revision from
* // the left SRF02 on the Bumper Board!
*
*/
uint8_t SRF02_getFirmware(uint8_t channel)
{
uint8_t adr = SRF02_i2cadr(channel);
I2CTWI_transmitByte(adr, SRF02_COMMAND);
return (I2CTWI_readByte(adr));
}
/**
* Sends a "ping" to the SRF02. This command starts
* a measurement or only sends an ultrasonic burst
* depending on mode.
*
* Input: channel -> [1..6] (for details see
* function SRF02_i2cadr()!)
* mode -> 80: Result in [inch]
* 81: Result in [cm]
* 82: Result in [us]
* 86: Result in [inch] *
* 87: Result in [cm] *
* 88: Result in [us] *
* 92: 40 kHz burst (no measurement)
* *) Fake ranging mode!
*
*/
void SRF02_ping(uint8_t channel, uint8_t mode)
{
I2CTWI_transmit2Bytes(SRF02_i2cadr(channel), SRF02_COMMAND, mode);
}
/**
* Reads and returns the SRF02 measurement result.
* The result unit [inch, cm, us] depends on the
* measuring mode (see SRF02_ping()!).
* The measurement result of the SRF02 sensor is
* also stored in distsrf02.
*
* Input: channel -> [1..6] (for details see
* function SRF02_i2cadr()!)
*
*/
uint16_t SRF02_read(uint8_t channel)
{
uint8_t adr = SRF02_i2cadr(channel);
I2CTWI_transmitByte(adr, SRF02_RANGE_MSB);
I2CTWI_readBytes(adr, registerBuf, 2);
distsrf02 = registerBuf[0] * 256 + registerBuf[1];
return distsrf02;
}
double distsrf02_l; // Left SRF02 distance [mm]
double distsrf02_r; // Right SRF02 distance [mm]
/**
* Calculates and returns the distance [mm] from
* a SRF02 result measured in microseconds [us]
* (modes 82 or 88).
*
* Input: us -> SRF02 result [us]
*
*/
double SRF02_calculate(uint16_t us)
{
double time_ms = us / 2000.0f; // One way [ms]
return (SONIC_SPEED * time_ms); // Distance [mm]
}
/**
* Performs a SRF02 measurement and returns the
* result [inch, cm, us] depending on mode.
* This function is BLOCKING for about 65 ms!
* If you don't want a blocking measurement, you
* have to write your own function with a non
* blocking pause between starting measurement
* and reading the result.
*
* Input: channel -> [1..6] (for details see
* function SRF02_i2cadr()!)
* mode -> 80: Result in [inch]
* 81: Result in [cm]
* 82: Result in [us]
*
*/
uint16_t SRF02_measure(uint8_t channel, uint8_t mode)
{
uint8_t adr = SRF02_i2cadr(channel);
SRF02_ping(adr, mode);
mSleep(65);
return (SRF02_read(adr));
}
/**
* Changes the SRF02 I2C slave address.
*
* Input: adr -> Old I2C address [0xe0..0xfe]
* newadr -> New I2C address [0xe0..0xfe]
*
* Hints: - adr and newadr may not be equal!
* - newadr must be even!
* - If you use this function, only !!ONE!!
* SRF02 (with the I2C address adr) may
* be connected to the I2C bus!
*
*/
void SRF02_changeAdr(uint8_t adr, uint8_t newadr)
{
if (adr == newadr) return;
if (newadr < 0xe0) return;
newadr &= 0xfe;
I2CTWI_transmit2Bytes(adr, SRF02_COMMAND, 160);
mSleep(50);
I2CTWI_transmit2Bytes(adr, SRF02_COMMAND, 170);
mSleep(50);
I2CTWI_transmit2Bytes(adr, SRF02_COMMAND, 165);
mSleep(50);
I2CTWI_transmit2Bytes(adr, SRF02_COMMAND, newadr);
}
#endif
// Radar sensors:
#ifdef RADAR_SENSORS
/**
* Call this once before using the Radar sensors.
*
*/
void RADARS_init(void)
{
IO_MULTIIO_SHARPS_PWR_DDR |= IO_MULTIIO_SHARPS_PWR_IN;
// Radar sensors power on:
IO_MULTIIO_SHARPS_PWR_PORT &= ~IO_MULTIIO_SHARPS_PWR_IN;
}
uint16_t adcradar_l; // Left Radar sensor ADC value
uint16_t adcradar_r; // Right Radar sensor ADC value
/**
* This function reads and returns the ADC value
* of the left or right Radar sensor. The value is
* also stored in adcradar_l or adcradar_r.
*
* Input: side -> 2 (LEFT) = Left Radar sensor
* 3 (RIGHT) = Right Radar sensor
*
*/
uint16_t getRADARSensor(uint8_t side)
{
if (side == LEFT) {
adcradar_l = readADC(ADC_MULTIIO_SHARP_L);
return adcradar_l;
}
if (side == RIGHT) {
adcradar_r = readADC(ADC_MULTIIO_SHARP_R);
return adcradar_r;
}
return 0;
}
double procradar_l; // Left Radar sensor result
double procradar_r; // Right Radar sensor result
/**
* Calculates and returns the left or right Radar
* sensor ADC result by using the data read from
* the Radar sensor with the function
* getRADARSensor().
*
* Input: side -> 2 (LEFT) = Left Radar sensor
* 3 (RIGHT) = Right Radar sensor
*
* Hints: - !This function is not ready for use!
* - Please add your processing of the
* ADC results here!
*
*/
double calculateRADAR(uint8_t side)
{
double vside;
if (side == LEFT) {
vside = adcradar_l * 5.0 / 1024.0;
//vside = ... // Process the Radar sensor ADC result!
return vside;
}
if (side == RIGHT) {
vside = adcradar_r * 5.0 / 1024.0;
//vside = ... // Process the Radar sensor ADC result!
return vside;
}
return 0;
}
/**
* Measures and returns the processed left or
* right Radar sensor result.
* The ADC value of the Radar sensor is also
* stored in adcradar_l or adcradar_r.
*
* Input: side -> 2 (LEFT) = Left Radar sensor
* 3 (RIGHT) = Right Radar sensor
*
*/
double measureRADAR(uint8_t side)
{
uint16_t tmp;
tmp = getRADARSensor(side);
return (calculateRADAR(side)); // Result
}
/**
* With this function you can switch the Radar
* sensors power on or off.
*
* Input: pwr -> 0 (false) = Radar sensors
* power off
* >0 (true) = Radar sensors
* power on
*
* Hint: If the Radar sensors are not used, you
* should always switch the Radar sensors
* power off to save energy!
*
*/
void setRADARSPower(uint8_t pwr)
{
if (pwr > 0)
IO_MULTIIO_SHARPS_PWR_PORT &= ~IO_MULTIIO_SHARPS_PWR_IN;
else
IO_MULTIIO_SHARPS_PWR_PORT |= IO_MULTIIO_SHARPS_PWR_IN;
}
#endif
// Bumper Board init:
/**
* Call this once before using the Bumper Board.
*
*/
void BUMPERBOARD_init(void)
{
LEDS_init();
#ifdef BUMPERS
MULTIIO_BUMPERS_init();
#endif
#ifdef SHARP_SENSORS
SHARPS_init();
#endif
#ifdef RADAR_SENSORS
RADARS_init();
#endif
}
/*****************************************************************************/
/*****************************************************************************/
// LFS and Bumper Board initialisation and shutdown:
/**
* You MUST call this function at the beginning of a
* main program, that uses the LFS or Bumper Board.
*
*/
void lfsbumper_init(void)
{
// LFS:
#ifdef LFS
LFS_init(); // Init LFS Board
#endif
// Bumper:
BUMPERBOARD_init(); // Init Bumper Board
}
/**
* If you don't use any function of the LFS and
* Bumper Board, you can put the LFS and Bumper
* Board into "SHUTDOWN MODE". In this mode the
* power consumption is very low to save energy.
*
*/
void lfsbumper_shutdown(void)
{
// LEDs:
setbumperLEDs(0b00); // Bumper LEDs off
// Bumper:
#ifdef SHARP_SENSORS
setSHARPSPower(0); // SHARPS/RADARS power off
#endif
#ifdef RADAR_SENSORS
setRADARSPower(0);
#endif
// LFS:
#ifdef LFS
setLFSPower(0); // LFS power off
#endif
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.2 (function SRF02_calculate bug fixed) 18.10.2014 by Dirk
* - v. 1.1 (adapted for use with I2CMasterLib) 09.05.2013 by Dirk
* - v. 1.0 (initial release) 28.03.2013 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Linienfolger Demo
Fast jeder mobile Roboter hat inzwischen Linienfolge-Sensoren (LFS). Der RP6 bekommt sie mit der Multi IO Projekt Platine jetzt auch. Das Liniensensor Board muss dazu an den 8-poligen Stecker "CNY70" auf der MultiIO angeschlossen und vorn am RP6 knapp über dem Boden montiert werden.
Gegenüber der Standardstellung der Jumper (siehe hier!) müssen Jumper auf der MultiIO für die LFS-Nutzung umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block.
Wahl-Jumper: Stellung der Jumper
Hinweis: Die Jumper sind orange eingezeichnet!
Zur Messung der 3,3V-Spannung und zur Auswertung des Touch-Sensors kann zusätzlich je ein Jumper auf 3V3-PF4 und NE5-PF3 gesetzt werden, wenn keine analogen Sensoren auf der Bumper-Platine benutzt werden.
makefile:
... TARGET = RP6M256_MultiIO_2 ... SRC += RP6M256_LFSBumperLib.c ...
Datei: RP6M256_MultiIO_2.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a second test for the MultiIO Project Board.
* It tests the Line Following Sensor (LFS) Board with 5 (3) CNY70s.
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 LFS Bumper library":
// (This is the library for accessing the LFS and Bumper Board!)
#include "RP6M256_LFSBumperLib.h"
/*****************************************************************************/
/**
* Write a floating point number to the WIFI.
*
* Example:
*
* // Write a floating point number to the WIFI (no exponent):
* writeDouble_WIFI(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString_WIFI(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 2!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", " Selftest 2");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
uint16_t lfs_l, lfs_m, lfs_r;
#ifdef FIVE_SENSORS
uint16_t lfs_l1, lfs_r1;
#endif
startStopwatch1();
// IMPORTANT:
lfsbumper_init(); // LFS & Bumper init!!!
//setServoPower(1); // Servo power ON!
while(true)
{
if(getStopwatch1() > 500) // 0.5s
{
// LFS ADC test:
#ifdef FIVE_SENSORS
lfs_l1 = getLFS(CH_LFS_L1);
writeString_WIFI("\nADC LFS_L1: ");
writeInteger_WIFI(lfs_l1, DEC);
#endif
lfs_l = getLFS(CH_LFS_L);
writeString_WIFI("\nADC LFS_L: ");
writeInteger_WIFI(lfs_l, DEC);
lfs_m = getLFS(CH_LFS_M);
writeString_WIFI("\nADC LFS_M: ");
writeInteger_WIFI(lfs_m, DEC);
lfs_r = getLFS(CH_LFS_R);
writeString_WIFI("\nADC LFS_R: ");
writeInteger_WIFI(lfs_r, DEC);
#ifdef FIVE_SENSORS
lfs_r1 = getLFS(CH_LFS_R1);
writeString_WIFI("\nADC LFS_R1: ");
writeInteger_WIFI(lfs_r1, DEC);
#endif
writeString_WIFI("\n");
setStopwatch1(0);
}
// task_I2CTWI();
}
return 0;
}
Erklärung
Bumper Demo
Fabian hat ein sehr universelles Bumper-Board designt, auf das 2 LEDs, 2 Bumper-Taster, zusätzlich 2 analoge SHARP-IR-Entfernungssensoren (in dieser Demo: GP2Y0A02YK), alternativ 2 I2C-Ultraschall-Entfernungssensoren (SRF02) oder 2 Radar-Sensoren (mit RSM-1650) montiert werden können. Das Bumper-Board muss an den 7-poligen Stecker "Bumper" der MultiIO angeschlossen und vorn oder hinten am RP6 montiert werden.
Diese Software demonstriert das Auslesen aller dieser Sensoren bis auf die Radar-Sensoren (weil ich die Radar-Sensoren noch nicht bekommen habe!).
Gegenüber der Standardstellung der Jumper (siehe hier!) müssen Jumper auf der MultiIO für die Nutzung des Bumper-Boards umgesteckt werden. Die folgenden Abbildungen zeigen den Wahl-Jumper-Block.
Wahl-Jumper: Stellung der Jumper für analoge Sensoren (SHARP, Radar)
Hinweis: Die Jumper sind orange eingezeichnet!
Diese Jumperstellung entspricht der Standard-Jumperstellung!
Wahl-Jumper: Stellung der Jumper für I2C-Sensoren (SRF02)
Hinweis: Die Jumper sind orange eingezeichnet!
Zur Messung der 3,3V-Spannung und zur Auswertung des Touch-Sensors kann zusätzlich je ein Jumper auf 3V3-PF4 und NE5-PF3 gesetzt werden. Alternativ kann diese Messung auch wie bei der Standard- Jumperstellung mit Jumpern auf 3V3-AD3 und NE5-AD1 erfolgen, wenn man die LFS-Kanäle L und M nicht braucht.
makefile:
... TARGET = RP6M256_MultiIO_3 ... SRC += RP6M256_LFSBumperLib.c ...
Datei RP6M256_MultiIO_3.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a third test for the MultiIO Project Board.
* It tests the Bumper Board with 2 LEDs, 2 bumpers, 2 analog SHARP IR
* distance sensors (GP2Y0A02YK) and/or 2 I2C ultrasonic distance sensors
* (SRF02) and/or 2 Radar sensors (RSM-1650 based, see here: http://www.
* jokesoft.de/weidmann/index.php?option=com_content&view=section&layout=
* blog&id=17&Itemid=21).
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 LFS Bumper library":
// (This is the library for accessing the LFS and Bumper Board!)
#include "RP6M256_LFSBumperLib.h"
/*****************************************************************************/
/**
* Write a floating point number to the WIFI.
*
* Example:
*
* // Write a floating point number to the WIFI (no exponent):
* writeDouble_WIFI(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString_WIFI(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 3!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", " Selftest 3");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
uint8_t i, bump_l, bump_r;
uint16_t distsrf_l, distsrf_r;
startStopwatch1();
// IMPORTANT:
lfsbumper_init(); // LFS & Bumper init!!!
//setServoPower(1); // Servo power ON!
// Bumper LEDs test:
for (i = 0; i < 10; i++) {
setbumperLEDL(1); // Left bumper LED on
setbumperLEDR(0); // Right bumper LED off
setLEDs(0b0101);
mSleep(500);
setbumperLEDs(0b10); // Right LED on, left LED off
setLEDs(0b1010);
mSleep(500);
}
setbumperLEDR(0); // Right bumper LED off
setLEDs(0b0000);
while(true)
{
if(getStopwatch1() > 1000) // 1s
{
#ifdef BUMPERS
// Bumpers test:
clearLCD();
bump_l = getMULTIIO_BUMPER(LEFT); // Read bumper
setCursorPosLCD(0, 0);
writeIntegerLCD(bump_l, DEC);
setCursorPosLCD(0, 4);
writeStringLCD("<BUMPER>");
bump_r = getMULTIIO_BUMPER(RIGHT);
setCursorPosLCD(0, 15);
writeIntegerLCD(bump_r, DEC);
#endif
#ifdef SHARP_SENSORS
// SHARP sensors test:
distsharp_l = measureSHARP(LEFT); // Measure
writeString_WIFI("\nSHARP SENSORS ->");
writeString_WIFI("\nLeft Distance: ");
writeDouble_WIFI(distsharp_l, 5, 1);
distsharp_r = measureSHARP(RIGHT);
writeString_WIFI("cm\nRight Distance: ");
writeDouble_WIFI(distsharp_r, 5, 1);
writeString_WIFI("cm\n");
#endif
#ifdef SRF02_SENSORS
// SRF02 sensors test:
distsrf_l = SRF02_measure(CH_SRF02_L, MODE_CM);
writeString_WIFI("\nSRF02 SENSORS ->");
writeString_WIFI("\nLeft Distance: ");
writeInteger_WIFI(distsrf_l, DEC);
distsrf_r = SRF02_measure(CH_SRF02_R, MODE_CM);
writeString_WIFI("cm\nRight Distance: ");
writeInteger_WIFI(distsrf_r, DEC);
writeString_WIFI("cm\n");
#endif
#ifdef RADAR_SENSORS
// Radar sensors test:
procradar_l = measureRADAR(LEFT); // Measure
writeString_WIFI("\nRADAR SENSORS ->");
writeString_WIFI("\nLeft Result: ");
writeDouble_WIFI(procradar_l, 5, 1);
writeString_WIFI("cm\nADC left Value: ");
writeInteger_WIFI(adcradar_l, DEC);
procradar_r = measureRADAR(RIGHT);
writeString_WIFI("\nRight Result: ");
writeDouble_WIFI(procradar_r, 5, 1);
writeString_WIFI("cm\nADC right Value: ");
writeInteger_WIFI(adcradar_r, DEC);
writeString_WIFI("\n");
#endif
setStopwatch1(0);
}
task_I2CTWI();
}
return 0;
}
Erklärung
Radio Board Library
Wird bei Bedarf DEN Usern zur Verfügung gestellt, die die Radio-Platine bestellt haben und mit ihr arbeiten möchten.
3,3V-Stromversorgung
Der I2C-AM/FM/SW/LW-Empfänger Si4735 wird mit einer Versorgungsspannung von 3,3V und an einem 3,3V-I2C-Bus betrieben. Diese Voraussetzungen sind gegeben, wenn der Empfänger an einen der beiden 5-poligen Anschluß-Stecker "3V3-I2C" auf der MultiIO angeschlossen wird. Allerdings muss noch die 3,3V-Stromversorgung der MultiIO eingeschaltet werden. Dazu sind zwei zusätzliche Jumper zu setzen, siehe folgende Abbildung:
Library Header
Library Source
Erklärung
Demo
Erklärung
Library für die Orientierung im Raum
"Orientierung im Raum" ist ein vielschichtiger Begriff. Für einen mobilen Roboter ist es wichtig, seine Neigung (Pitch, Roll) zu kennen und ggf. seine Fahrtrichtung. In geschlossenen Räumen sollte er sich ebenfalls orientieren können, Hindernisse erkennen und z.B. eine Karte seiner Umgebung erstellen können.
In dieser Library soll es um das Auslesen von drei Sensoren gehen, die dabei helfen, sich zu orientieren:
- GPS-Empfänger (NL-552ETTL)
- 2D-Kompass (HDMM01)
- 9D-IMU (MinIMU-9 v2)
Achsen der Kompasssensoren
Die Library für die Orientierung im Raum geht davon aus, dass der Kompasssensor HDMM01 und die minIMU-9 v2 in einer bestimmten Ausrichtung auf der MultiIO bzw. auf dem RP6 befestigt sind. Folgende Abbildung zeigt diese Anordnung:
Wichtige Hinweise
- Zur Befestigung der Kompasssensoren sollte man keine ferromagnetischen Teile benutzen.
- Diese Sensoren müssen so angebracht werden, dass es keine Achsenabweichung zum RP6 gibt. Das heißt: Die Sensor-Platinen müssen genau parallel zur Fläche der Hauptplatine des RP6 ausgerichtet sein und ihre Kanten parallel zu den Kanten der RP6 Hauptplatine.
- Der Abstand der Kompasssensoren zur MultiIO Platine (oder zu anderen Teilen des RP6) sollte mehr als 5 cm betragen.
Diese Avr-gcc Library für das Multi IO Projekt Board (= "MultiIO") geht von folgenden Voraussetzungen aus:
- Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt.
- Die M256 ist der I2C-Bus Master.
- Die I2C-Bus Geschwindigkeit beträgt 100 kHz.
- Alle Jumper auf der MultiIO sind in ihrer Standardstellung (1).
- Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden.
- Der Wannenstecker IO_Mxxx der MultiIO ist mit dem Wannenstecker IO_PWM/T2/T3 der M256 1:1 verbunden.
Zu (1): Siehe hier!
UART-Anschluß
Auf der Multi IO Projekt Platine kann am Anschluß "GPS" ein GPS-Empfänger (z.B. NL-552ETTL) angeschlossen werden. Dieses Modul ist der einzige direkt an die MultiIO anschließbare externe Sensor, der seine Daten über eine serielle Schnittstelle zur Verfügung stellt. Mit seinen Pins TX (Ausgang) und RX (Eingang) muss der NL-552ETTL mit einem (Hardware-) UART des Microcontrollers auf der M256 (z.B. UART1) verbunden werden. In der Standard-Konfiguration der MultiIO (siehe hier!) ist diese Verbindung NICHT vorgesehen.
Wie kann man diese Verbindung herstellen?
Eine Verbindung zwischen dem GPS-Modul auf der MultiIO und dem UART1 der M256 läßt sich wie folgt herstellen:
1. Anstelle des Wannensteckers IO_PWM/T2/T3 verbindet man den Wannenstecker UART_SPI1/T5 der M256 mit IO-Mxxx der MultiIO. Damit ist der UART1-Anschluß problemlos erreicht und das GPS-Modul sofort einsatzfähig.
Nachteil: Man muss die Multi IO Library ändern, um sie an die
anderen Portpins dieses Steckers anzupassen.
2. Man verbindet mit einem 2-poligen Kabel die beiden Pins "Tx" und "Rx5V" des 3-poligen Steckers "P_GPS1" direkt oberhalb des GPS-Modul-Anschlusses auf der MultiIO so mit dem Wannenstecker UART_SPI1/T5 der M256:
| GPS-Funktion | P_GPS1-Pin | M256-Port | M256-Funktion | UART_SPI1/T5-Pin | Bemerkung |
| Tx | 2 | PD2 | RXD1 | 8 | Tx hat 3,3V-Pegel! |
| Rx5V | 1 | PD3 | TXD1 | 6 | Kann auch entfallen! * |
Zu *) Diese Verbindung wird nur zur Konfiguration des GPS-Moduls
benötigt.
Wenn das Modul konfiguriert ist, kann Rx5V des GPS-Moduls
ohne Anschluß bleiben!
WICHTIGER HINWEIS:
Wenn die UART1-Verbindung zwischen GPS-Modul und M256 über den 3-poligen Stecker "P_GPS1" hergestellt wird, sollten die Tx- und Rx5V-Leitungen des GPS-Moduls von IO-Mxxx abgetrennt werden. Dazu müssen die zwei Jumper "Tx2" und "Rx" der Standard-Jumperstellung (siehe hier!) an Pins 6 und 8 vom Jumper-Block "J_IO" abgezogen werden. Siehe folgende Abbildung:
Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!
Die Library (Software-Bibliothek) besteht aus drei Teilen:
- Dem Configuration Header -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der Orientation Library dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss.
- Dem Library Header -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library.
- Der Library Source -> Das ist die eigentliche Library.
Eingebunden wird auch die RP6M256uart1 Library, mit der ein Auslesen des GPS-Moduls über UART1 der M256 möglich ist, und der MultiIO Configuration Header. Die RP6M256uart1_Library gehört in das Verzeichnis RP6Lib/RP6control_M256_WIFI.
Configuration Header
Vor dem Kompilieren der Library muss eine Anpassung des Configuration Headers an die eigenen Hardware-Voraussetzungen erfolgen. Dazu gibt es im Configuration Header drei Definitionen:
- GPS -> Das GPS-Modul (NL-552ETTL) wird benutzt
- COMPASS_2D -> Der 2D-Compass (HDMM01) wird benutzt
- IMU_9D -> Die 9D-IMU (MinIMU-9 v2) wird verwendet
Im nachfolgenden Listing ist nur die Definition IMU_9D aktiv. Dies muss an die eigene Hardware angepaßt werden. Wenn z.B. die IMU NICHT benutzt werden soll, muss die Zeile mit "IMU_9D" auskommentiert werden. Dazu setzt man "//" an den Zeilenanfang. Beispiel:
//#define IMU_9D // 9D-IMU (MinIMU-9 v2) is used
Wenn der 2D-Compass (HDMM01) benutzt wird, muss man zunächst den Sensor kalibrieren. Dazu müssen die Minimal- und Maximalwerte der beiden Sensor-Achsen ermittelt und in die Definitionen MAX_X_2DM, MIN_X_2DM, MAX_Y_2DM, MIN_Y_2DM eingetragen werden. Dazu geht man so vor:
- Den RP6 mit montiertem HDMM01 auf eine waagerechte Fläche stellen,- am besten genau dort, wo er später herumfahren und sich mithilfe des Kompasses orientieren soll. Es dürfen keine Elektrogeräte, Metallteile und stromführende Leitungen in der Nähe sein, die das Magnetfeld verändern können.
- Die Demo starten und die X-Achsen und Y-Achsen Ausgaben im Wifi-Terminal oder auf dem LCD beobachten.
- Den RP6 langsam um die eigene (Z-)Achse drehen und dabei die Minimal- und Maximalwerte der X- und Y-Achse ermitteln.
- Die 4 Werte in den Configuration Header eintragen.
- Die Demo neu kompilieren.
Wenn die 9D-IMU (MinIMU-9 v2) benutzt wird, muss man alle drei Komponenten (Gyrosensor, Beschleunigungssensor, Magnetfeldsensor) der IMU kalibrieren. Dazu geht man so vor:
- Gyrosensor:
- Wenn der RP6 ruhig auf einer ebenen Fläche steht und nicht bewegt wird, sollten die Werte der X-, Y- und Z-Achse um den Nullpunkt schwanken. Tatsächlich wird es Abweichungen zum Nullwert geben. Zur Kompensierung der Null-Abweichung kann man die Definitionen OFFSET_X, OFFSET_Y und OFFSET_Z verwenden, um die Achsen zu "nullen". Zeigt die X-Achse in Ruhe z.B. 756 an, trägt man für OFFSET_X -756 ein.
- Da die Werte der 3 Achsen begrenzt werden, muss man die Minimal- und Maximalwerte der X-, Y- und Z-Achse festlegen und in die Definitionen MAX_X, MIN_X, MAX_Y, MIN_Y eintragen. Für den Anfang kann man die Werte bei +-20000 belassen.
- Wenn man den Temperaturwert des L3GD20 nutzen will, muss man die Definition GET_TEMP aktivieren (Standard!). Den Temperaturwert kann man noch eichen, indem man die Definition OFFSET_TEMP anpasst (ganzzahliger Wert!). Die Einheit von OFFSET_TEMP ist °C.
- Beschleunigungssensor:
- Die X-Achse wird kalibriert, indem man den RP6 mit der rechten Seite nach unten hält und den Maximalwert in MAX_X_A einträgt. Dann neigt man ihn auf seine linke Seite und trägt den negativen Maximalwert in MIN_X_A ein.
- Die Y-Achse wird kalibriert, indem man den RP6 mit der Vorderseite nach oben hält und den Maximalwert in MAX_Y_A einträgt. Dann kippt man die Vorderseite nach unten und trägt den negativen Maximalwert in MIN_Y_A ein.
- Die Z-Achse wird kalibriert, indem man den RP6 normal waagerecht hält und den Maximalwert in MAX_Z_A einträgt. Dann dreht man den RP6 "auf den Kopf" und trägt den negativen Maximalwert in MIN_Z_A ein.
- Sollten die angezeigten Pitch- und Roll-Werte nach der Kalibrierung der Achsen und genau waagerecht stehendem Sensor bzw. RP6 vom Nullwert deutlich abweichen, kann man sie mithilfe der Definitionen OFFSET_PITCH_A und OFFSET_ROLL_A "nullen". Die Einheit dieser Definitionen ist °.
HINWEIS: Die internen Berechnungen werden durch diese Offset-Korrektur NICHT beeinflußt. Die Offset-Werte können also nicht dazu benutzt werden, eine Achsenabweichung zwischen Sensor und RP6 zu kompensieren!
- Magnetfeldsensor:
- Zur Kalibrierung des Magnetfeldsensors stellt man den RP6 auf eine waagerechte Fläche. Es dürfen keine Elektrogeräte, Metallteile und stromführende Leitungen in der Nähe sein, die das Magnetfeld verändern können.
- Zunächst ist es möglich, die sogenannten "hard iron" Effekte auszugleichen. Dies sind feste Magnetfeldabweichungen, die durch das "Gerät" (in unserem Fall also durch den kompletten RP6) verursacht werden, in oder auf dem der Sensor angebracht ist. Mit den Definitionen OFFSET_X_M, OFFSET_Y_M und OFFSET_Z_M können diese Effekte ausgeglichen werden. Diese Kalibrierung wird hier aktuell (noch) nicht beschrieben, so dass man die Werte für diese Definitionen zunächst bei 0 belassen sollte.
- Die X-Achse und Y-Achse wird kalibriert, indem man den RP6 langsam mehrfach um die eigene (Z-)Achse dreht und die Maximalwerte in MAX_X_M und MAX_Y_M einträgt. Die negativen Maximalwerte trägt man in MIN_X_M und MIN_Y_M ein.
- Die Z-Achse wird kalibriert, indem man den RP6 auf die rechte oder linke Seite legt, ihn dann langsam mehrfach auf der Stelle (um seine Y-Achse) dreht und den Maximalwert in MAX_Z_M und den negativen Maximalwert in MIN_Z_M einträgt.
- Mit der Definition DECLINATION_M kann man zur Verbesserung der Genauigkeit der Richtungsangaben den magnetischen Deklinationswert [°] (d.h. die magnetische Missweisung) am aktuellen Standort festlegen. Hier befindet sich ein "Deklinations-Rechner", mit dem man den Deklinationswert für den eigenen Standort ermitteln kann.
- Wenn man den Temperaturwert des LSM303DLHC nutzen will, muss man die Definition GET_TEMP_M aktivieren (Standard!). Den Temperaturwert kann man noch eichen, indem man die Definition OFFSET_TEMP_M anpasst. Die Einheit von OFFSET_TEMP_M ist °C.
HINWEISE ZUR KALIBRIERUNG:
- Das Demo-Programm ist nicht sehr gut geeignet, um die Kalibrierungen durchzuführen.
- Man sollte mit einem eigenen kurzen Kalibrierungs-Programm nur die Rohwerte der jeweiligen X-, Y- und Z-Achse z.B. zweimal pro Sekunde ausgeben.
- Die Rohwerte sind in Variablen x_axis..., y_axis..., z_axis... enthalten. Da die Rohwerte in den Funktionen normalizeHDMM01(), normalizeL3GD20(), normalizeLSM303DLHC_A() bzw. normalizeLSM303DLHC_M() ggf. verändert werden, müssen die Rohwerte VOR Durchlaufen dieser Funktionen ausgegeben werden.
- Ohne eine sorgfältige Kalibrierung wird die Kompass-Funktion (insbesondere die neigungskompensierte Ausgabe der Himmelsrichtung) nicht funktionieren!
- Nach dem Eintragen der Kalibrierungs-Werte in den Configuration Header (RP6M256_Orientation.h) muss die Demo neu kompiliert werden. Vorher sollten die Compiler-Hilfsdateien der Orientierungs-Library (Dateiendungen .st und .o) im Programmverzeichnis gelöscht werden.
- Getestet wird das Ergebnis der Kalibrierung, indem die Ausgabewerte (Temperaturen, Pitch, Roll, Heading, Neigungs-kompensiertes Heading) der Demo überprüft werden. Dazu kann man Vergleichswerte (Thermometer, Winkelmesser für Pitch, Roll und externer Kompass für Heading) heranziehen.
- Wenn die Ausgabewerte nicht den Erwartungen entsprechen, muss die Kalibrierung wiederholt werden.
- Eine Kalibrierung der Sensoren ist nur zuverlässig, solange die Position des jeweiligen Sensors und seine komplette Hardware-Umgebung (also der RP6 mit allen seinen Teilen!) nicht verändert wird. Gibt es trotzdem Veränderungen, z.B. durch weitere Aufbauten auf dem RP6, müssen diese Sensoren neu kalibriert werden.
Datei: RP6M256_Orientation.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples * **************************************************************************** * Example: RP6M256 Orientation Library * Author(s): Dirk * **************************************************************************** * Description: * Configuration header file for new Orientation library. * * **************************************************************************** */ #ifndef RP6M256_ORIENTATION_H #define RP6M256_ORIENTATION_H /*****************************************************************************/ // The following sensors are used: // - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS // chipset // - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG // 2-axis magnetometer // - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and // the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer /*****************************************************************************/ // Define the devices that are connected to the MultiIO Project Board here! // Make a comment of a line belonging to a sensor that is NOT attached to // the MultiIO Board! //#define GPS // GPS module (NL-552ETTL) is used //#define COMPASS_2D // 2D-Compass module (HDMM01) is used #define IMU_9D // 9D-IMU (MinIMU-9 v2) is used /*****************************************************************************/ // GPS (NL-552ETTL): /*****************************************************************************/ // 2D-Compass (HDMM01): // MMC2120MG magnetometer: #define I2C_MULTIIO_HDMM01_ADR 0x60 // Default // MMC2120MG magnetometer calibration data: #define MAX_X_2DM 2194.0 // Max. X-axis value #define MIN_X_2DM 2026.0 // Min. X-axis value #define BIAS_X_2DM (MAX_X_2DM + MIN_X_2DM) / 2.0 #define SF_X_2DM 2.0 / (MAX_X_2DM - MIN_X_2DM) #define MAX_Y_2DM -1892.0 // Max. Y-axis value #define MIN_Y_2DM -2068.0 // Min. Y-axis value #define BIAS_Y_2DM (MAX_Y_2DM + MIN_Y_2DM) / 2.0 #define SF_Y_2DM 2.0 / (MAX_Y_2DM - MIN_Y_2DM) /*****************************************************************************/ // 9D-IMU (MinIMU-9 v2): // L3GD20 gyroscope: #define I2C_MULTIIO_L3GD20_ADR 0xd6 // Default (SDO -> VCC) //#define I2C_MULTIIO_L3GD20_ADR 0xd4 // SDO -> GND // L3GD20 gyroscope calibration data: #define MAX_X 20000 // Max. X-axis value #define MIN_X -20000 // Min. X-axis value #define OFFSET_X 0 // Offset X-axis #define MAX_Y 20000 // Max. Y-axis value #define MIN_Y -20000 // Min. Y-axis value #define OFFSET_Y 0 // Offset Y-axis #define MAX_Z 20000 // Max. Z-axis value #define MIN_Z -20000 // Min. Z-axis value #define OFFSET_Z 0 // Offset Z-axis // L3GD20 temperature sensor definitions: #define GET_TEMP // Use temperature sensor #define OFFSET_TEMP 0 // Temperature offset [°C] // LSM303DLHC accelerometer: #define I2C_MULTIIO_LSM303DLHC_A_ADR 0x32 // Default // LSM303DLHC accelerometer calibration data: #define MAX_X_A 1019 // Max. X-axis value #define MIN_X_A -1057 // Min. X-axis value #define MAX_Y_A 1029 // Max. Y-axis value #define MIN_Y_A -1064 // Min. Y-axis value #define MAX_Z_A 965 // Max. Z-axis value #define MIN_Z_A -1057 // Min. Z-axis value #define OFFSET_PITCH_A 0.0 // Offset Pitch [°] #define OFFSET_ROLL_A 0.0 // Offset Roll [°] // LSM303DLHC magnetometer: #define I2C_MULTIIO_LSM303DLHC_M_ADR 0x3c // Default // LSM303DLHC magnetometer calibration data: #define MAX_X_M 420.0 // Max. X-axis value #define MIN_X_M -94.0 // Min. X-axis value #define OFFSET_X_M 0 // Hard iron X-axis offset #define MAX_Y_M 341.0 // Max. Y-axis value #define MIN_Y_M -125.0 // Min. Y-axis value #define OFFSET_Y_M 0 // Hard iron Y-axis offset #define MAX_Z_M 467.0 // Max. Z-axis value #define MIN_Z_M -30.0 // Min. Z-axis value #define OFFSET_Z_M 0 // Hard iron Z-axis offset #define DECLINATION_M 0.0 // E at local position [°] // LSM303DLHC temperature sensor definitions: #define GET_TEMP_M // Enable temperature sensor #define OFFSET_TEMP_M 0.0 // Temperature offset [°C] /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_OrientationLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Header
Datei: RP6M256_OrientationLib.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples * **************************************************************************** * Example: RP6M256 Orientation Library * Author(s): Dirk * **************************************************************************** * Description: * Header file for new Orientation library. * * **************************************************************************** */ #ifndef RP6M256_ORIENTATIONLIB_H #define RP6M256_ORIENTATIONLIB_H /*****************************************************************************/ // The following sensors are used: // - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS // chipset // - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG // 2-axis magnetometer // - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and // the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer /*****************************************************************************/ // Includes: #include "RP6M256Lib.h" #include "RP6I2CmasterTWI.h" #include "RP6M256_Orientation.h" #include "RP6M256uart1.h" // The RP6 M256 UART1 Library #include "RP6M256_MultiIO.h" #include <math.h> /*****************************************************************************/ // GPS (NL-552ETTL): #ifdef GPS extern char parseBuffer[256]; void GPS_init(void); void clearParseBuffer(void); uint8_t parseNMEAsentence(char *sentence, uint8_t data); uint8_t checkNMEAsentence(void); void parseNMEAitem(char *result, uint8_t index); #endif /*****************************************************************************/ // 2D-Compass (HDMM01): // MMC2120MG magnetometer internal registers: #define INTERNAL_REGISTER 0x00 #define MSB_X_AXIS 0x01 #define LSB_X_AXIS 0x02 #define MSB_Y_AXIS 0x03 #define LSB_Y_AXIS 0x04 // MMC2120MG magnetometer internal register settings: #define INTERNAL_REGISTER_TM 0b00000001 #define INTERNAL_REGISTER_SET 0b00000010 #define INTERNAL_REGISTER_RESET 0b00000100 #ifdef COMPASS_2D extern uint8_t intreg2dm; extern int16_t x_axis2dm, y_axis2dm; extern int16_t heading2dm; extern double x2dm, y2dm; void resetCoilHDMM01(void); void COMPASS_2D_init(void); uint8_t readHDMM01(void); void normalizeHDMM01(void); int16_t headingHDMM01(void); #endif /*****************************************************************************/ // 9D-IMU (MinIMU-9 v2): // L3GD20 gyroscope internal registers: #define WHO_AM_I 0x0f #define CTRL_REG1 0x20 #define CTRL_REG2 0x21 #define CTRL_REG3 0x22 #define CTRL_REG4 0x23 #define CTRL_REG5 0x24 #define REFERENCE 0x25 #define OUT_TEMP 0x26 #define STATUS_REG 0x27 #define OUT_X_L 0x28 #define OUT_X_H 0x29 #define OUT_Y_L 0x2a #define OUT_Y_H 0x2b #define OUT_Z_L 0x2c #define OUT_Z_H 0x2d #define FIFO_CTRL_REG 0x2e #define FIFO_SRC_REG 0x2f #define INT1_CFG 0x30 #define INT1_SRC 0x31 #define INT1_TSH_XH 0x32 #define INT1_TSH_XL 0x33 #define INT1_TSH_YH 0x34 #define INT1_TSH_YL 0x35 #define INT1_TSH_ZH 0x36 #define INT1_TSH_ZL 0x37 #define INT1_DURATION 0x38 // L3GD20 gyroscope register settings: #define CTRL_REG1_DEFAULT 0b00000000 // Gyro Power-down #define CTRL_REG1_SLEEP 0b00001000 // Gyro Sleep #define CTRL_REG1_EN 0b00001111 // Gyro on, all axes enable // LSM303DLHC accelerometer internal registers: #define CTRL_REG1_A 0x20 #define CTRL_REG2_A 0x21 #define CTRL_REG3_A 0x22 #define CTRL_REG4_A 0x23 #define CTRL_REG5_A 0x24 #define CTRL_REG6_A 0x25 #define REFERENCE_A 0x26 #define STATUS_REG_A 0x27 #define OUT_X_L_A 0x28 #define OUT_X_H_A 0x29 #define OUT_Y_L_A 0x2a #define OUT_Y_H_A 0x2b #define OUT_Z_L_A 0x2c #define OUT_Z_H_A 0x2d #define FIFO_CTRL_REG_A 0x2e #define FIFO_SRC_REG_A 0x2f #define INT1_CFG_A 0x30 #define INT1_SOURCE_A 0x31 #define INT1_THS_A 0x32 #define INT1_DURATION_A 0x33 #define INT2_CFG_A 0x34 #define INT2_SOURCE_A 0x35 #define INT2_THS_A 0x36 #define INT2_DURATION_A 0x37 #define CLICK_CFG_A 0x38 #define CLICK_SRC_A 0x39 #define CLICK_THS_A 0x3a #define TIME_LIMIT_A 0x3b #define TIME_LATENCY_A 0x3c #define TIME_WINDOW_A 0x3d // LSM303DLHC accelerometer register settings: #define CTRL_REG1_A_10HZ 0b00100111 // 10Hz & all axes enable #define CTRL_REG1_A_50HZ 0b01000111 // 50Hz & all axes enable #define CTRL_REG4_A_DEFAULT 0b00000000 // Default (Normal mode) #define CTRL_REG4_A_HR 0b00001000 // HR output mode // LSM303DLHC magnetometer internal registers: #define CRA_REG_M 0x00 #define CRB_REG_M 0x01 #define MR_REG_M 0x02 #define OUT_X_H_M 0x03 #define OUT_X_L_M 0x04 #define OUT_Z_H_M 0x05 #define OUT_Z_L_M 0x06 #define OUT_Y_H_M 0x07 #define OUT_Y_L_M 0x08 #define SR_REG_MG 0x09 #define IRA_REG_M 0x0a #define IRB_REG_M 0x0b #define IRC_REG_M 0x0c #define TEMP_OUT_H_M 0x31 #define TEMP_OUT_L_M 0x32 // LSM303DLHC magnetometer register settings: #define CRA_REG_M_15HZ 0b00010000 // Default (Data rate 15Hz) #define CRA_REG_M_15HZ_T_EN 0b10010000 // 15Hz & temperature enable #define CRB_REG_M_13GAUSS 0b00100000 // Gain +-1.3Gauss #define MR_REG_M_CCM 0b00000000 // Continuous-conversion mode #define MR_REG_M_SCM 0b00000001 // Single-conversion mode #define MR_REG_M_SLEEP 0b00000011 // Default (Sleep-mode) #ifdef IMU_9D extern int16_t x_axisg, y_axisg, z_axisg; extern double xg, yg, zg; #ifdef GET_TEMP extern int8_t temperatureg; #endif extern int16_t x_axisa, y_axisa, z_axisa; extern double pitch, roll; extern double xa, ya, za; extern double the, phi; extern int16_t x_axism, y_axism, z_axism; extern int16_t headingm; extern double xm, ym, zm; #ifdef GET_TEMP_M extern int16_t temperaturem; extern double temperature_imu; #endif extern int16_t headingtc; void L3GD20_init(void); void readL3GD20(void); void normalizeL3GD20(void); #ifdef GET_TEMP int16_t map(int16_t, int16_t, int16_t, int16_t, int16_t); #define calcTempL3GD20(__VALUE__) map(__VALUE__,127,-128,-40,85) #endif void LSM303DLHC_A_init(void); void readLSM303DLHC_A(void); void normalizeLSM303DLHC_A(void); void positionLSM303DLHC_A(void); void LSM303DLHC_M_init(void); void readLSM303DLHC_M(void); void normalizeLSM303DLHC_M(void); int16_t headingLSM303DLHC_M(void); int16_t headingLSM303DLHC_TC(void); void IMU_9D_init(void); #endif /*****************************************************************************/ // Orientation library initialisation: void orientation_init(void); /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_OrientationLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Source
Datei: RP6M256_OrientationLib.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 Orientation Library
* Author(s): Dirk
* ****************************************************************************
* Description:
*
* This is our new Library that contains basic routines and functions for
* accessing certain sensors connected to the MultiIO Project Board.
*
* ****************************************************************************
*/
/*****************************************************************************/
// The following sensors are used:
// - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS
// chipset
// - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG
// 2-axis magnetometer
// - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and
// the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer
/*****************************************************************************/
// Includes:
#include "RP6M256_OrientationLib.h"
/*****************************************************************************/
// GPS (NL-552ETTL):
#ifdef GPS
/**
* ---------------------------------------------------------------------------
* NMEA 0183 short description:
* 1. GPRMC:
* $GPRMC,213938.000,A,5108.3356,N,00951.0774,E,0.00,79.64,061012,,,A*53
* Index: 1 2 3 4 5 6 7 8 9 12CS
* 1 = UTC of position [hhmmss.sss]
* 2 = Data status (A=ok, V=void)
* 3 = Latitude of fix [ddmm.mmmm]
* 4 = N or S
* 5 = Longitude of fix [dddmm.mmmm]
* 6 = E or W
* 7 = Speed over ground in knots
* 8 = Track made good, degrees true
* 9 = UT date [ddmmyy]
* 10 = Magnetic variation degrees (Easterly var. subtracts from true course)
* 11 = E or W
* 12 = Mode indicator (NMEA V2.3):
* A=Autonomous mode
* D=Differential mode
* E=Estimated (dead-reckoning) mode
* M=Manual input mode
* S=Simulated mode
* N=Data not valid
* CS = Checksum
*
* 2. GPVTG:
* $GPVTG,79.64,T,,M,0.00,N,0.0,K,A*31
* Index: 1 2 4 5 6 7 8 9 CS
* 1 = Track made good
* 2 = Fixed text 'T' indicates that track made good is relative to true north
* 3 = Track degrees
* 4 = M=Magnetic
* 5 = Speed over ground in knots
* 6 = Fixed text 'N' indicates that speed over ground is in knots
* 7 = Speed over ground in kilometers/hour
* 8 = Fixed text 'K' indicates that speed over ground is in kilometers/hour
* 9 = Mode indicator (A,D,E,M,S,N)
* CS = Checksum
*
* 3. GPGGA:
* $GPGGA,213938.000,5108.3356,N,00951.0774,E,1,04,5.3,158.5,M,46.2,M,,0000*52
* Index: 1 2 3 4 5 6 7 8 9 11 12 14 CS
* 1 = UTC of position
* 2 = Latitude of fix
* 3 = N or S
* 4 = Longitude of fix
* 5 = E or W
* 6 = GPS quality indicator:
* 0=invalid
* 1=GPS fix (SPS)
* 2=DGPS fix
* 3=PPS fix
* 4=Real Time Kinematic
* 5=Float RTK
* 6=estimated (dead reckoning) (NMEA V2.3)
* 7=Manual input mode
* 8=Simulation mode
* 7 = Number of satellites in use [not those in view]
* 8 = Horizontal dilution of position (HDOP)
* 9 = Antenna altitude above/below mean sea level (geoid)
* 10 = Meters (Antenna height unit)
* 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and
* mean sea level. -=geoid is below WGS-84 ellipsoid)
* 12 = Meters (Units of geoidal separation)
* 13 = Age in seconds since last update from diff. reference station
* 14 = Differential reference station ID#
* CS = Checksum
*
* 4. GPGSA:
* $GPGSA,A,3,02,04,10,07,,,,,,,,,7.4,5.3,5.1*33
* Index: 1 2 3 4 5 6 15 16 17 CS
* 1 = Selection mode:
* M=Manual, forced to operate in 2D or 3D
* A=Automatic 2D/3D
* 2 = Mode:
* 1=no fix
* 2=2D
* 3=3D
* 3-14 = IDs of SVs used in position fix (null for unused fields)
* 15 = PDOP
* 16 = HDOP
* 17 = VDOP
* CS = Checksum
*
* 5. GPGSV:
* $GPGSV,3,1,12,02,36,301,37,04,53,235,21,05,03,294,,07,35,170,29*79
* $GPGSV,3,2,12,08,04,187,28,10,56,290,36,13,88,106,20,16,12,076,*7D
* $GPGSV,3,3,12,20,23,118,11,23,50,067,,29,03,351,12,30,16,048,22*70
* Index: 1 2 3 4 5 6 7 8 9 10 12 13 14 15 16 17 18 19 CS
* 1 = Total number of messages of this type in this cycle
* 2 = Message number
* 3 = Total number of SVs in view
* 4 = SV PRN number
* 5 = Elevation in degrees, 90 maximum
* 6 = Azimuth, degrees from true north, 000 to 359
* 7 = SNR, 00-99 dB (null when not tracking)
* 8-11 = Information about second SV, same as field 4-7
* 12-15= Information about third SV, same as field 4-7
* 16-19= Information about fourth SV, same as field 4-7
* CS = Checksum
*
* 6. GPGLL:
* $GPGLL,5108.3356,N,00951.0774,E,213938.000,A,A*5C
* Index: 1 2 3 4 5 6 7 CS
* 1 = Latitude of fix
* 2 = N or S
* 3 = Longitude of fix
* 4 = E or W
* 5 = UTC of position
* 6 = Data status (A=ok, V=void)
* 7 = Mode indicator (A,D,E,M,S,N)
* CS = Checksum
*
* COMMENTS:
* a) Each sentence (line) begins with '$'.
* b) Each sentence (line) ends with '*', followed by the checksum (two
* chars building an 8-bit HEX number) and terminated by <CR><LF>.
* c) The checksum (CS) is calculated by XOR of all chars between '$' and '*'.
* d) The max. sentence (line) length is 82 chars (including <CR><LF>).
* e) Further infos:
* -> http://www.nmea.org/
* -> http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm
* -> http://de.wikipedia.org/wiki/NMEA_0183
* -> http://aprs.gids.nl/nmea/
* ---------------------------------------------------------------------------
*
* Call this once before using the GPS.
*
*/
void GPS_init(void)
{
cli(); // Disable global interrupts
// Initialisation of UART1:
UBRR1H = UBRR_BAUD_LOW >> 8; // Setup UART1: Baud is 38.4 kBaud
UBRR1L = (uint8_t) UBRR_BAUD_LOW;
UCSR1A = 0x00;
UCSR1C = (0<<UMSEL10) | (0<<UMSEL11) | (1<<UCSZ11) | (1<<UCSZ10);
UCSR1B = (1 << TXEN1) | (1 << RXEN1) | (1 << RXCIE1);
clearReceptionBuffer1();
sei(); // Enable global interrupts
}
char parseBuffer[256];
uint16_t parseBuffer_length = 0;
uint8_t parseBuffer_pos = 0;
/**
* Clears the parseBuffer
*
*/
void clearParseBuffer(void)
{
parseBuffer_pos = 0;
parseBuffer_length = 0;
}
/**
* Writes received byte data into buffer parseBuffer and returns if a full
* NMEA sentence has been received. Then the buffer is terminated with 0.
* Returns 0 if a sentence has been detected and buffer is ready.
* Returns 1 if no sentence has been received yet
* Returns 2 if buffer overflow occured
*
*/
uint8_t parseNMEAsentence(char *sentence, uint8_t data)
{
static uint8_t startflag = false;
static char last_data[10];
uint16_t length = 0;
uint8_t i = 0;
if(data == 0) return 1;
while (sentence[i++]) length++;
if(length > 10) length = 10;
for(i = 0; i < 9; i++) last_data[i] = last_data[i + 1];
last_data[9] = data;
parseBuffer[parseBuffer_pos] = data;
parseBuffer_pos++;
parseBuffer_length++;
if(parseBuffer_pos > 254) {
clearParseBuffer();
startflag = false;
return 2;
}
if(!startflag && (strstr(last_data, sentence) != NULL)) { // Chosen sentence start
for(i = 0; i < length; i++) {
parseBuffer[i] = sentence[i];
}
parseBuffer_pos = length;
parseBuffer_length = length;
startflag = true;
}
if(startflag) {
if(data == '\r') { // Chosen sentence end
parseBuffer[parseBuffer_pos] = 0;
last_data[9] = 0;
startflag = false;
return 0;
}
}
return 1;
}
/**
* Outputs true, if the NMEA sentence in parseBuffer is intact,
* else returns false.
*
*/
uint8_t checkNMEAsentence(void)
{
uint8_t i, cs;
char cs_str[3];
if(parseBuffer_length < 18) return false;
if(parseBuffer_length > 82) return false;
if(parseBuffer[0] != '$') return false;
if(parseBuffer[parseBuffer_length - 4] != '*') return false;
cs = parseBuffer[1];
for(i = 2; i < (parseBuffer_length - 4); i++) {
cs ^= parseBuffer[i]; // Calculate checksum
}
itoa(cs, &cs_str[0], HEX);
strupr(&cs_str[0]); // cs as uppercase chars
if((cs_str[0] != parseBuffer[parseBuffer_length - 3])
|| (cs_str[1] != parseBuffer[parseBuffer_length - 2])) return false;
return true;
}
/**
* Outputs a result string containing the data with index of the NMEA
* sentence in parseBuffer.
*
*/
void parseNMEAitem(char *result, uint8_t index)
{
uint8_t index_cnt, buffer_idx, result_idx;
index_cnt = index;
buffer_idx = 6;
while(index_cnt) {
if(parseBuffer[buffer_idx++] == ',') index_cnt--;
}
result_idx = 0;
while ((parseBuffer[buffer_idx] != ',')
&& (parseBuffer[buffer_idx] != '*')) {
result[result_idx++] = parseBuffer[buffer_idx++];
}
result[result_idx] = 0;
}
#endif
/*****************************************************************************/
// 2D-Compass (HDMM01):
#ifdef COMPASS_2D
uint8_t intreg2dm;
int16_t x_axis2dm, y_axis2dm;
int16_t heading2dm;
double x2dm, y2dm;
/**
* This function sets and resets the internal coil of the
* magnetic sensor.
*
*/
void resetCoilHDMM01(void)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER, INTERNAL_REGISTER_SET);
mSleep(1);
I2CTWI_transmit2Bytes(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER, INTERNAL_REGISTER_RESET);
mSleep(5);
}
/**
* Call this once before using the 2D-Compass.
*
*/
void COMPASS_2D_init(void)
{
// Reset coil:
resetCoilHDMM01();
}
/**
* This function reads the X-axis and Y-axis values from the
* HDMM01 and stores them in the global variables x_axis2dm
* and y_axis2dm.
* It returns the Internal register of the HDMM01.
*
*/
uint8_t readHDMM01(void)
{
uint8_t readBuf[5];
I2CTWI_transmit2Bytes(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER, INTERNAL_REGISTER_TM);
mSleep(5);
I2CTWI_transmitByte(I2C_MULTIIO_HDMM01_ADR, INTERNAL_REGISTER);
I2CTWI_readBytes(I2C_MULTIIO_HDMM01_ADR, readBuf, 5); // Read Int. reg. & X-/Y-axis
// xb = y:
x_axis2dm = ((readBuf[MSB_Y_AXIS] & 0x0f) << 8) + readBuf[LSB_Y_AXIS];
// yb = -x:
y_axis2dm = ((readBuf[MSB_X_AXIS] & 0x0f) << 8) + readBuf[LSB_X_AXIS];
y_axis2dm *= -1;
return readBuf[INTERNAL_REGISTER];
}
/**
* This function scales the x-axis2dm and y-axis2dm
* values read from the HDMM01 with the function
* readHDMM01() and stores them in the global double
* variables x2dm and y2dm.
*
*/
void normalizeHDMM01(void)
{
x2dm = (x_axis2dm - BIAS_X_2DM) * SF_X_2DM;
y2dm = (y_axis2dm - BIAS_Y_2DM) * SF_Y_2DM;
}
/**
* This function calculates the heading by using the
* normalized sensor values x2dm and y2dm (see function
* normalizeHDMM01()!). The heading is a value from 0°
* to 359°. If the robot's front (and the sensor's
* SDA/SCL pin side) points to ...
* north -> heading is 0°,
* east -> heading is 90°,
* south -> heading is 180°,
* west -> heading is 270°.
*
*/
int16_t headingHDMM01(void)
{
double hdg = atan2(y2dm, x2dm) * 180 / M_PI;
if(hdg < 0.0) hdg += 360.0;
return ((int16_t) hdg);
}
#endif
/*****************************************************************************/
// 9D-IMU (MinIMU-9 v2):
#ifdef IMU_9D
// --------------------------------------------
// L3GD20 gyroscope functions:
int16_t x_axisg, y_axisg, z_axisg;
double xg, yg, zg;
#ifdef GET_TEMP
int8_t temperatureg;
#endif
/**
* Call this once before using the 3D-Gyro.
*
*/
void L3GD20_init(void)
{
// Set normal power mode, all axes enabled:
I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG1, CTRL_REG1_EN);
mSleep(10);
}
/**
* This function reads the X-axis, Y-axis, Z-axis and
* temperature values from the L3GD20 gyroscope and stores
* them in the global variables x_axisg, y_axisg, z_axisg
* and temperatureg.
*
*/
void readL3GD20(void)
{
uint8_t readBuf[6];
I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80));
I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read X-/Y-/Z-axis
// xb = y:
x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L];
// yb = -x:
y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L];
y_axisg *= -1;
// zb = z:
z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L];
mSleep(10);
#ifdef GET_TEMP
// Read temperature raw value:
I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, OUT_TEMP);
temperatureg = I2CTWI_readByte(I2C_MULTIIO_L3GD20_ADR);
mSleep(10);
#endif
}
/**
* This function adds zero offsets and limits the x-axisg,
* y-axisg and z-axisg values read from the L3GD20
* gyroscope with the function readL3GD20() and stores them
* in the global double variables xg, yg and zg.
*
*/
void normalizeL3GD20(void)
{
// Add zero offsets:
x_axisg += OFFSET_X;
y_axisg += OFFSET_Y;
z_axisg += OFFSET_Z;
// Limit raw values:
if (x_axisg < MIN_X) x_axisg = MIN_X;
if (x_axisg > MAX_X) x_axisg = MAX_X;
if (y_axisg < MIN_Y) y_axisg = MIN_Y;
if (y_axisg > MAX_Y) y_axisg = MAX_Y;
if (z_axisg < MIN_Z) z_axisg = MIN_Z;
if (z_axisg > MAX_Z) z_axisg = MAX_Z;
xg = (double) x_axisg;
yg = (double) y_axisg;
zg = (double) z_axisg;
}
#ifdef GET_TEMP
/**
* Returns the integer mapping function result.
*
* Input: value -> Source value
* fromSource -> Low source limit
* toSource -> High source limit
* fromTarget -> Low target limit
* toTarget -> High target limit
*
* There is also a macro calcTempL3GD20(), which
* calculates the temperature [°C] from the L3GD20
* temperature output value (temperatureg) using this
* mapping function.
*
* Example:
* tmp = calcTempL3GD20(temperatureg);
* // This calculates the temperature [°C]!
*
*/
int16_t map(int16_t value, int16_t fromSource, int16_t toSource, int16_t fromTarget, int16_t toTarget)
{
double temp;
temp = ((double) (value - fromSource) / (toSource - fromSource) \
* (toTarget - fromTarget) + fromTarget);
return (int16_t) temp;
}
#endif
// --------------------------------------------
// LSM303DLHC accelerometer functions:
int16_t x_axisa, y_axisa, z_axisa;
double pitch, roll;
double xa, ya, za;
double the, phi;
/**
* Call this once before using the 3D-Accelerometer.
*
*/
void LSM303DLHC_A_init(void)
{
// Set normal power mode, 10 Hz data rate, all axes enabled:
I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_A_ADR, CTRL_REG1_A, CTRL_REG1_A_10HZ);
mSleep(10);
// Set HD mode:
I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_A_ADR, CTRL_REG4_A, CTRL_REG4_A_HR);
mSleep(10);
}
/**
* This function reads the X-axis, Y-axis and Z-axis values
* from the LSM303DLHC accelerometer and stores them in the
* global variables x_axisa, y_axisa and z_axisa.
*
*/
void readLSM303DLHC_A(void)
{
uint8_t readBuf[6];
I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_A_ADR, (OUT_X_L_A | 0x80));
I2CTWI_readBytes(I2C_MULTIIO_LSM303DLHC_A_ADR, readBuf, 6); // Read X-/Y-/Z-axis
// xb = -y:
x_axisa = ((readBuf[OUT_Y_H_A - OUT_X_L_A] << 8) + readBuf[OUT_Y_L_A - OUT_X_L_A]) / 16;
x_axisa *= -1;
// yb = x:
y_axisa = ((readBuf[OUT_X_H_A - OUT_X_L_A] << 8) + readBuf[OUT_X_L_A - OUT_X_L_A]) / 16;
// zb = -z:
z_axisa = ((readBuf[OUT_Z_H_A - OUT_X_L_A] << 8) + readBuf[OUT_Z_L_A - OUT_X_L_A]) / 16;
z_axisa *= -1;
mSleep(10);
}
/**
* This function limits and normalizes the x-axisa,
* y-axisa and z-axisa values read from the LSM303DLHC
* accelerometer with the function readLSM303DLHC_A() and
* stores them in the global double variables xa, ya and
* za.
* It also calculates the position in space in form of the
* tilt angles (the = pitch, phi = roll) and stores them
* in the global double variables the and phi.
*
*/
void normalizeLSM303DLHC_A(void)
{
// Limit raw values:
if (x_axisa < MIN_X_A) x_axisa = MIN_X_A;
if (x_axisa > MAX_X_A) x_axisa = MAX_X_A;
if (y_axisa < MIN_Y_A) y_axisa = MIN_Y_A;
if (y_axisa > MAX_Y_A) y_axisa = MAX_Y_A;
if (z_axisa < MIN_Z_A) z_axisa = MIN_Z_A;
if (z_axisa > MAX_Z_A) z_axisa = MAX_Z_A;
xa = (double) x_axisa / 1000.0;
ya = (double) y_axisa / 1000.0;
za = (double) z_axisa / 1000.0;
// Calculate Pitch and Roll:
the = asin(-xa); // Calculate the and phi
phi = asin(ya / cos(the)); // from the raw values
}
/**
* This function calculates the position in space by
* using the the and phi values (see function
* normalizeLSM303DLHC_A()!). It stores the position
* in the global double variables pitch and roll.
*
*/
void positionLSM303DLHC_A(void)
{
pitch = the * 180 / M_PI + OFFSET_PITCH_A;
roll = phi * 180 / M_PI + OFFSET_ROLL_A;
}
// --------------------------------------------
// LSM303DLHC magnetometer functions:
int16_t x_axism, y_axism, z_axism;
int16_t headingm;
double xm, ym, zm;
#ifdef GET_TEMP_M
int16_t temperaturem;
double temperature_imu;
#endif
int16_t headingtc;
/**
* Call this once before using the 3D-Compass.
*
*/
void LSM303DLHC_M_init(void)
{
#ifdef GET_TEMP_M
// Set Data output rate to 15Hz, enable temperature sensor:
I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ_T_EN);
#else
// Set Data output rate to 15Hz:
I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ);
#endif
mSleep(10);
// Set Gain to +-1.3Gauss:
I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, CRB_REG_M, CRB_REG_M_13GAUSS);
mSleep(10);
// Set Continuous-conversion mode:
I2CTWI_transmit2Bytes(I2C_MULTIIO_LSM303DLHC_M_ADR, MR_REG_M, MR_REG_M_CCM);
mSleep(10);
}
/**
* This function reads the X-axis, Y-axis, Z-axis and
* temperature values from the LSM303DLHC magnetometer
* and stores them in the global variables x_axism,
* y_axism, z_axism and temperaturem.
*
*/
void readLSM303DLHC_M(void)
{
uint8_t readBuf[6];
I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_M_ADR, OUT_X_H_M);
I2CTWI_readBytes(I2C_MULTIIO_LSM303DLHC_M_ADR, readBuf, 6); // Read X-/Y-/Z-axis
// xb = y:
x_axism = (readBuf[OUT_Y_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Y_L_M - OUT_X_H_M];
// yb = x:
y_axism = (readBuf[OUT_X_H_M - OUT_X_H_M] << 8) + readBuf[OUT_X_L_M - OUT_X_H_M];
// zb = -z:
z_axism = (readBuf[OUT_Z_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Z_L_M - OUT_X_H_M];
z_axism *= -1;
mSleep(10);
#ifdef GET_TEMP_M
// Read temperature raw value:
I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_M_ADR, TEMP_OUT_H_M);
readBuf[0] = I2CTWI_readByte(I2C_MULTIIO_LSM303DLHC_M_ADR);
I2CTWI_transmitByte(I2C_MULTIIO_LSM303DLHC_M_ADR, TEMP_OUT_L_M);
readBuf[1] = I2CTWI_readByte(I2C_MULTIIO_LSM303DLHC_M_ADR);
temperaturem = ((readBuf[0] << 8) + readBuf[1]) / 16;
mSleep(10);
#endif
}
/**
* This function compensates hard iron effects and
* scales the x-axism, y-axism and z-axism values read
* from the LSM303DLHC magnetometer with the function
* readLSM303DLHC_M() and stores them in the global
* double variables xm, ym and zm.
*
*/
void normalizeLSM303DLHC_M(void)
{
// Add hard iron offsets:
x_axism += OFFSET_X_M;
y_axism += OFFSET_Y_M;
z_axism += OFFSET_Z_M;
// Scale raw values:
xm = (x_axism - MIN_X_M) / (MAX_X_M - MIN_X_M) * 2 - 1;
ym = (y_axism - MIN_Y_M) / (MAX_Y_M - MIN_Y_M) * 2 - 1;
zm = (z_axism - MIN_Z_M) / (MAX_Z_M - MIN_Z_M) * 2 - 1;
}
/**
* This function calculates the heading by using the
* normalized sensor values xm and ym (see function
* normalizeLSM303DLHC_M()!). The heading is a value
* from 0° to 359°. If the robot's front (and the
* sensor's upper edge with view on the printed
* side) points to ...
* north -> heading is 0°,
* east -> heading is 90°,
* south -> heading is 180°,
* west -> heading is 270°.
*
*/
int16_t headingLSM303DLHC_M(void)
{
double hdg = atan2(ym, xm) * 180 / M_PI + DECLINATION_M;
if(hdg < 0.0) hdg += 360.0;
return ((int16_t) hdg);
}
// --------------------------------------------
/**
* This function calculates the TILT COMPENSATED
* heading by using accelerometer (the, phi) AND
* magnetometer (xm, ym, zm) data. The heading is a
* value from 0° to 359°. If the robot's front (and
* the sensor's upper edge with view on the printed
* side) points to ...
* north -> heading is 0°,
* east -> heading is 90°,
* south -> heading is 180°,
* west -> heading is 270°.
*
*/
int16_t headingLSM303DLHC_TC(void)
{
double hdgtc, xh, yh;
xh = xm * cos(the) + zm * sin(the);
yh = xm * sin(phi) * sin(the) + ym * cos(phi) - zm * sin(phi) * cos(the);
// zh = -xm * cos(phi) * sin(the) + ym * sin(phi) + zm * cos(phi) * cos(the);
hdgtc = atan2(yh, xh) * 180 / M_PI + DECLINATION_M;
if(hdgtc < 0.0) hdgtc += 360.0;
return ((int16_t) hdgtc);
}
// --------------------------------------------
/**
* Call this once before using the 9D-IMU.
*
*/
void IMU_9D_init(void)
{
L3GD20_init(); // Gyroscope init
LSM303DLHC_A_init(); // Accelerometer init
LSM303DLHC_M_init(); // Magnetometer init
}
#endif
/*****************************************************************************/
/*****************************************************************************/
// Orientation library initialisation:
/**
* Call this once before using the GPS module,
* the 2D-Compass or the 9D-IMU.
*
*/
void orientation_init(void)
{
#ifdef GPS
GPS_init();
#endif
#ifdef COMPASS_2D
COMPASS_2D_init();
#endif
#ifdef IMU_9D
IMU_9D_init();
#endif
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.1 (smaller bugs fixed) 18.07.2013 by Dirk
* - v. 1.0 (initial release) 14.04.2013 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6M256_MultiIO_05 ... SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256uart1.c SRC += RP6M256_OrientationLib.c ...
Datei: RP6M256_MultiIO_05.c
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a fifth test for the MultiIO Project Board.
* It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU).
*
* The following sensors are used:
* - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS
* chipset
* - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG
* 2-axis magnetometer
* - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and
* the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
#include "RP6M256uart1.h" // The RP6 M256 UART1 Library
#include <math.h>
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Orientation library":
// (This is the library for accessing certain sensors connected to the
// MultiIO Project Board!)
#include "RP6M256_OrientationLib.h"
/*****************************************************************************/
// Variables:
uint8_t ch;
char item[12];
char dir[3];
/*****************************************************************************/
/**
* Returns a 2 character string for the eighth
* parts of the direction calculated from the
* heading value.
*
* Input: heading -> Heading value [0..359]
*
*/
void calculateDir(char *dir, uint16_t heading)
{
dir[1] = ' ';
dir[2] = '\0';
if ((heading <= 22) || (heading >=338)) dir[0] = 'N';
if ((heading >= 23) && (heading <= 67)) {dir[0] = 'N'; dir[1] = 'E';}
if ((heading >= 68) && (heading <= 112)) dir[0] = 'E';
if ((heading >= 113) && (heading <= 157)) {dir[0] = 'S'; dir[1] = 'E';}
if ((heading >= 158) && (heading <= 202)) dir[0] = 'S';
if ((heading >= 203) && (heading <= 247)) {dir[0] = 'S'; dir[1] = 'W';}
if ((heading >= 248) && (heading <= 292)) dir[0] = 'W';
if ((heading >= 293) && (heading <= 337)) {dir[0] = 'N'; dir[1] = 'W';}
}
/**
* Write a floating point number to the WIFI.
*
* Example:
*
* // Write a floating point number to the WIFI (no exponent):
* writeDouble_WIFI(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString_WIFI(&buffer[0]);
}
/**
* Write a floating point number to the LCD.
*
* Example:
*
* // Write a floating point number to the LCD (no exponent):
* writeDoubleLCD(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", " Selftest 5");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
startStopwatch1();
// IMPORTANT:
orientation_init(); // Orientation init!
while(true)
{
if(getStopwatch1() > 3000) // 3s
{
#ifdef GPS
// GPS test:
clearParseBuffer();
clearReceptionBuffer1();
writeString_P_WIFI("\nGPS SENSOR ->\n");
while(parseNMEAsentence("$GPGGA", ch)) {
ch = readChar1();
}
writeString_P_WIFI("GPGGA NMEA sentence:\n");
writeString_WIFI(parseBuffer);
writeChar_WIFI('\n');
if(checkNMEAsentence()) {
writeString_P_WIFI("Sentence OK!\n");
setCursorPosLCD(0, 0); // line 1
parseNMEAitem(item, 3);
writeStringLCD(item);
writeString_P_WIFI("Position ");
writeString_WIFI(item);
parseNMEAitem(item, 2);
writeStringLCD(item);
writeString_WIFI(item);
writeChar_WIFI('\n');
writeCharLCD('m');
parseNMEAitem(item, 9);
writeStringLCD(item);
writeString_P_WIFI("Height [m]: ");
writeString_WIFI(item);
writeChar_WIFI('\n');
setCursorPosLCD(1, 0); // line 2
parseNMEAitem(item, 5);
writeStringLCD(item);
writeString_P_WIFI("Position ");
writeString_WIFI(item);
parseNMEAitem(item, 4);
writeStringLCD(item);
writeString_WIFI(item);
writeChar_WIFI('\n');
writeCharLCD('s');
parseNMEAitem(item, 7);
writeStringLCD(item);
writeString_P_WIFI("Satellites: ");
writeString_WIFI(item);
writeChar_WIFI('\n');
writeCharLCD('q');
parseNMEAitem(item, 6);
writeStringLCD(item);
}
else {
writeString_P_WIFI("Sentence NOT OK!\n");
showScreenLCD("Sentence NOT OK!", "----------------");
}
mSleep(3000);
#endif
#ifdef COMPASS_2D
// 2D-Compass test:
task_I2CTWI();
intreg2dm = readHDMM01(); // Get sensor values
task_I2CTWI();
writeString_P_WIFI("\n2D-Compass SENSOR ->\n");
writeString_P_WIFI("Internal register: ");
writeIntegerLength_WIFI(intreg2dm, BIN, 8);
writeChar_WIFI('\n');
if(!intreg2dm) { // Data valid?
writeString_P_WIFI("X-axis: ");
writeInteger_WIFI(x_axis2dm, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Y-axis: ");
writeInteger_WIFI(y_axis2dm, DEC);
writeChar_WIFI('\n');
normalizeHDMM01(); // Normalize data
heading2dm = headingHDMM01(); // Calculate heading
writeString_P_WIFI("Heading [°]: ");
writeInteger_WIFI(heading2dm, DEC);
calculateDir(dir, heading2dm);
writeString_P_WIFI(" Direction: ");
writeString_WIFI(dir);
writeChar_WIFI('\n');
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("Heading ");
writeIntegerLCD(heading2dm, DEC);
writeStringLCD_P(" ");
writeStringLCD(dir);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("X ");
writeIntegerLCD(x_axis2dm, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 8); // line 2 pos 9
writeStringLCD_P("Y ");
writeIntegerLCD(y_axis2dm, DEC);
writeStringLCD_P(" ");
}
task_I2CTWI();
mSleep(3000);
#endif
#ifdef IMU_9D
// 9D-IMU test:
// L3GD20 gyroscope:
task_I2CTWI();
readL3GD20(); // Get sensor values
normalizeL3GD20();
task_I2CTWI();
writeString_P_WIFI("\n3D-Gyro SENSOR ->\n");
writeString_P_WIFI("X-axis: ");
writeInteger_WIFI(x_axisg, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Y-axis: ");
writeInteger_WIFI(y_axisg, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Z-axis: ");
writeInteger_WIFI(z_axisg, DEC);
writeChar_WIFI('\n');
#ifdef GET_TEMP
temperatureg = calcTempL3GD20(temperatureg) + 5;
temperatureg += OFFSET_TEMP;
writeString_P_WIFI("Temperature [°]: ");
writeInteger_WIFI(temperatureg, DEC);
writeChar_WIFI('\n');
#endif
// LSM303DLHC accelerometer:
task_I2CTWI();
readLSM303DLHC_A(); // Get sensor values
task_I2CTWI();
writeString_P_WIFI("\n3D-Acceleration SENSOR ->\n");
writeString_P_WIFI("X-axis: ");
writeInteger_WIFI(x_axisa, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Y-axis: ");
writeInteger_WIFI(y_axisa, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Z-axis: ");
writeInteger_WIFI(z_axisa, DEC);
writeChar_WIFI('\n');
normalizeLSM303DLHC_A(); // Normalize data
positionLSM303DLHC_A(); // Calculate position
writeString_P_WIFI("POSITION: \n");
writeString_P_WIFI(" Pitch [°]: ");
writeDouble_WIFI(pitch, 6, 1);
writeChar_WIFI('\n');
writeString_P_WIFI(" Roll [°]: ");
writeDouble_WIFI(roll, 6, 1);
writeChar_WIFI('\n');
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("P");
writeDoubleLCD(pitch, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8); // line 1 pos 9
writeStringLCD_P("R");
writeDoubleLCD(roll, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("X");
writeIntegerLCD(x_axisa, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 5); // line 2 pos 6
writeStringLCD_P("Y");
writeIntegerLCD(y_axisa, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 10); // line 2 pos 11
writeStringLCD_P("Z");
writeIntegerLCD(z_axisa, DEC);
writeStringLCD_P(" ");
task_I2CTWI();
mSleep(3000);
// LSM303DLHC magnetometer:
task_I2CTWI();
readLSM303DLHC_M(); // Get sensor values
task_I2CTWI();
writeString_P_WIFI("\n3D-Compass SENSOR ->\n");
writeString_P_WIFI("X-axis: ");
writeInteger_WIFI(x_axism, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Y-axis: ");
writeInteger_WIFI(y_axism, DEC);
writeChar_WIFI('\n');
writeString_P_WIFI("Z-axis: ");
writeInteger_WIFI(z_axism, DEC);
writeChar_WIFI('\n');
normalizeLSM303DLHC_M(); // Normalize data
headingm = headingLSM303DLHC_M(); // Calculate heading
writeString_P_WIFI(" Heading [°]: ");
writeInteger_WIFI(headingm, DEC);
calculateDir(dir, headingm);
writeString_P_WIFI(" Direction: ");
writeString_WIFI(dir);
writeChar_WIFI('\n');
headingtc = headingLSM303DLHC_TC(); // Calculate TILT COMPENSATED
writeString_P_WIFI("TILT COMPENSATED heading [°]: "); // heading
writeInteger_WIFI(headingtc, DEC);
calculateDir(dir, headingtc);
writeString_P_WIFI(" Direction: ");
writeString_WIFI(dir);
writeChar_WIFI('\n');
#ifdef GET_TEMP_M
temperature_imu = (double) temperaturem / 8.0 + OFFSET_TEMP_M;
writeString_P_WIFI("Temperature [°]: ");
writeDouble_WIFI(temperature_imu, 7, 2);
writeChar_WIFI('\n');
#endif
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("Heading ");
writeIntegerLCD(headingm, DEC);
writeStringLCD_P(" ");
writeStringLCD(dir);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("X");
writeIntegerLCD(x_axism, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 5); // line 2 pos 6
writeStringLCD_P("Y");
writeIntegerLCD(y_axism, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 10); // line 2 pos 11
writeStringLCD_P("Z");
writeIntegerLCD(z_axism, DEC);
writeStringLCD_P(" ");
task_I2CTWI();
#endif
setStopwatch1(0);
}
task_I2CTWI();
}
return 0;
}
Erklärung
GYRO Demo 1
Die Demo für die Orientation Library zeigt als Ausgaben des 3D-Gyros nur die Werte der 3 Achsen. Mit dem GYRO ist natürlich viel mehr möglich. Diese 1. GYRO Demo gibt fortlaufend die Winkelgeschwindigkeiten auf dem LCD aus, mit denen der RP6 bewegt wird.
makefile:
... TARGET = RP6M256_MultiIO_05_GYRO_01 ... SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256uart1.c SRC += RP6M256_OrientationLib.c ...
Datei: RP6M256_MultiIO_05_GYRO_01.c
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a fifth test for the MultiIO Project Board.
* It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU).
*
* The following sensors are used:
* - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS
* chipset
* - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG
* 2-axis magnetometer
* - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and
* the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
#include "RP6M256uart1.h" // The RP6 M256 UART1 Library
#include <math.h>
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Orientation library":
// (This is the library for accessing certain sensors connected to the
// MultiIO Project Board!)
#include "RP6M256_OrientationLib.h"
/*****************************************************************************/
// Defines:
//#define CALIBRATION
/*****************************************************************************/
// Variables:
double xdpsg = 0.0;
double ydpsg = 0.0;
double zdpsg = 0.0;
double last_xg = 0.0;
double last_yg = 0.0;
double last_zg = 0.0;
/*****************************************************************************/
#define CTRL_REG5_FIFO_EN 0b01000000 // FIFO on
#define FIFO_CTRL_REG_BYPASS 0b00000000 // Bypass mode (default)
#define FIFO_CTRL_REG_FIFO 0b00100000 // FIFO mode
#define FIFO_CTRL_REG_STREAM 0b01000000 // Stream mode
#define FIFO_CTRL_REG_STREAM2FIFO 0b01100000 // Stream-to-FIFO mode
#define FIFO_CTRL_REG_BYPASS2STREAM 0b10000000 // Bypass-to-Stream mode
/**
* Call this once in order to use the 3D-Gyro
* with the FIFO in Stream mode.
*
* Hint: Interrupts or FIFO watermark infos are
* NOT used here!
*
*/
void L3GD20_initStreamMode(void)
{
// Choose Stream mode:
I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, FIFO_CTRL_REG, FIFO_CTRL_REG_STREAM);
mSleep(10);
// Activate FIFO:
I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG5, CTRL_REG5_FIFO_EN);
mSleep(10);
}
#define DPS_PER_DIGIT 0.00875 // @ +-250 dps (default)
//#define DPS_PER_DIGIT 0.0175 // @ +-500 dps
//#define DPS_PER_DIGIT 0.07 // @ +-2000 dps
/**
* This is the L3GD20 gyroscope task.
*
* The task stores the averaged dps values in
* the global double variables xdpsg, ydpsg,
* zdpsg.
*
* ATTENTION: Stopwatch 7 is used for the GYRO
* task! Please do not use this
* stopwatch elsewhere in your
* program!
*
* Hint: You must calibrate the gyro carefully
* before using this task!
*
*/
void task_GYRO(void)
{
uint8_t readBuf[6];
if(getStopwatch7() > 100) // 100ms
{
I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80));
I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read X-/Y-/Z-axis
// xb = y:
x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L];
// yb = -x:
y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L];
y_axisg *= -1;
// zb = z:
z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L];
normalizeL3GD20();
xg *= DPS_PER_DIGIT;
yg *= DPS_PER_DIGIT;
zg *= DPS_PER_DIGIT;
xdpsg = (last_xg + xg) / 2;
ydpsg = (last_yg + yg) / 2;
zdpsg = (last_zg + zg) / 2;
last_xg = xg;
last_yg = yg;
last_zg = zg;
setStopwatch7(0);
}
}
/*****************************************************************************/
/**
* Write a floating point number to the LCD.
*
* Example:
*
* // Write a floating point number to the LCD (no exponent):
* writeDoubleLCD(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5 GYRO!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", "Selftest 5 GYRO");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
clearLCD();
startStopwatch1();
startStopwatch7(); // Used for GYRO task!
// IMPORTANT:
orientation_init(); // Orientation init!
L3GD20_initStreamMode(); // Gyro Stream mode init!
while(true)
{
#ifndef CALIBRATION
task_I2CTWI();
task_GYRO();
#endif
task_I2CTWI();
if(getStopwatch1() > 500) // 500ms
{
#ifdef CALIBRATION
// GYRO calibration part:
readL3GD20(); // Get sensor values
//normalizeL3GD20();
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeIntegerLCD(x_axisg, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8); // line 1 pos 9
writeStringLCD_P("Y");
writeIntegerLCD(y_axisg, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("Z");
writeIntegerLCD(z_axisg, DEC);
writeStringLCD_P(" ");
#else
// Display part:
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeDoubleLCD(xdpsg, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8); // line 1 pos 9
writeStringLCD_P("Y");
writeDoubleLCD(ydpsg, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("Z");
writeDoubleLCD(zdpsg, 6, 1);
writeStringLCD_P(" ");
#endif
setStopwatch1(0);
}
}
return 0;
}
Erklärung
GYRO Demo 2
Die Demo für die Orientation Library zeigt als Ausgaben des 3D-Gyros nur die Werte der 3 Achsen. Mit dem GYRO ist natürlich viel mehr möglich. Diese 2. GYRO Demo gibt fortlaufend die aufsummierten Winkel einer Drehung auf dem LCD aus.
In Ruhelage des RP6 sollten die Werte bei 0 bleiben oder nur ganz langsam über die Zeit "wegdriften". Dieses Driften ist einerseits eine (schlechte) Gyro-Eigenschaft, die hoch temperaturabhängig ist und sich nicht beeinflussen läßt. Andererseits: Wenn die Werte schnell (innerhalb weniger Sekunden) wegdriften, dann ist die Kalibrierung nicht sorgfältig genug gemacht worden. Wenn man das nicht besser hinkriegt, kann man auch den Wert von OFFSET_ZERO (aktuell 75) weiter erhöhen. Besser ist aber IMMER die gute Kalibrierung, weil durch die Erhöhung von OFFSET_ZERO die "Empfindlichkeit" für sehr langsame Drehungen geringer wird.
Kalibrierung des GYRO mit der Demo 2
Man kann die Kalibrierung des Gyro auch gut mit dieser Demo 2 machen: Wenn man in der Demo die "//" vor #define CALIBRATION wegmacht, kann man mit der Demo 2 die Rohwerte ausgeben.
1. In RP6M256_Orientation.h: a) #define IMU_9D aktivieren, die beiden anderen Definitionen (GPS, COMPASS_2D) auskommentieren b) #define GET_TEMP auskommentieren: //#define GET_TEMP c) OFFSET_X, OFFSET_Y, OFFSET_Z auf 0 setzen d) Datei speichern 2. Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien der Demo VORHER LÖSCHEN!) 3. In Ruhelage des RP6 die Werte beobachten: Schwanken sie z.B. bei X zwischen +20 und +140, also im Mittel um den Wert +80, dann in OFFSET_X -80 eintragen. Das so für alle 3 Achsen machen. Noch ein Beispiel: Schwanken die Werte bei Y zwischen -50 und +30, also im Mittel um -10, dann in OFFSET_Y +10 eintragen. Am Ende RP6M256_Orientation.h speichern. 4. In der Demo die "//" vor normalizeL3GD20(); wegmachen 5. Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien der Demo VORHER LÖSCHEN!) 6. Ausgabewerte ansehen: Sie müßten jetzt um den Nullpunkt schwanken,- jedenfalls viel besser um 0 gemittelt sein. Gibt es da noch konstante Abweichungen, dann mit Punkt 3. weitermachen, dabei die neuen Offsets nur zu den vorhandenen addieren, also nicht bei 0 wieder anfangen. (Dabei bleibt die Zeile normalizeL3GD20(); immer mit drin!) Das so lange machen, bis man zufrieden ist, dann Kalibrierung ENDE!!! 7. Jetzt KANN es noch mit der Demo GYRO_02 ans "Feintuning" gehen: a) In der Demo die "//" vor #define CALIBRATION wieder hinmachen (Messbetrieb, keine Kalibrierung mehr!). b) Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien der Demo VORHER LÖSCHEN!) c) Idealerweise bleiben in Ruhelage des RP6 jetzt alle Achsen dauerhaft bei 0.0, dann Feintuning ENDE!!! d) Ist das nicht der Fall und wandert eine Anzeige langsam immer weiter in den negativen oder positiven Bereich, kann man in der RP6M256_Orientation.h die OFFSET_X, _Y, _Z Werte vorsichtig korrigieren: Ich würde um max. +- 5 korrigieren, die Datei speichern und bei 7.b) weiter machen.
Noch ein Hinweis für ganz Exakte: Man kann jetzt noch den Wert OFFSET_ZERO der Demo kleiner machen (z.B. auf 60 oder 50 reduzieren). Dann driften die schön kalibrierten Werte evtl. wieder weg und man kann bei 7.b) weiter machen. ENDE des Feintuning [;-)] unter folgenden Bedingungen (was eher zutrifft):
- Die Korrektur der OFFSET_X, _Y, _Z Werte erfolgt im Wechsel um + 1 oder - 1.
- OFFSET_ZERO ist kleiner als 50.
- Das Wegdriften der Winkel-Werte beginnt erst nach 1/2 Stunde.
- Das Feintuning dauert insgesamt länger als 3 Stunden.
- Die Maximalzahl der Schreibzyklen auf den Flash-Speicher des Microcontrollers ist überschritten.
- Man vernachlässigt seine sonstigen Aufgaben im Leben.
makefile:
... TARGET = RP6M256_MultiIO_05_GYRO_02 ... SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256uart1.c SRC += RP6M256_OrientationLib.c ...
Datei: RP6M256_MultiIO_05_GYRO_02.c
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a fifth test for the MultiIO Project Board.
* It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU).
*
* The following sensors are used:
* - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS
* chipset
* - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG
* 2-axis magnetometer
* - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and
* the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
#include "RP6M256uart1.h" // The RP6 M256 UART1 Library
#include <math.h>
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Orientation library":
// (This is the library for accessing certain sensors connected to the
// MultiIO Project Board!)
#include "RP6M256_OrientationLib.h"
/*****************************************************************************/
// Defines:
//#define CALIBRATION
/*****************************************************************************/
// Variables:
double xdg = 0.0;
double ydg = 0.0;
double zdg = 0.0;
/*****************************************************************************/
#define CTRL_REG5_FIFO_EN 0b01000000 // FIFO on
#define FIFO_CTRL_REG_BYPASS 0b00000000 // Bypass mode (default)
#define FIFO_CTRL_REG_FIFO 0b00100000 // FIFO mode
#define FIFO_CTRL_REG_STREAM 0b01000000 // Stream mode
#define FIFO_CTRL_REG_STREAM2FIFO 0b01100000 // Stream-to-FIFO mode
#define FIFO_CTRL_REG_BYPASS2STREAM 0b10000000 // Bypass-to-Stream mode
/**
* Call this once in order to use the 3D-Gyro
* with the FIFO in Stream mode.
*
* Hint: Interrupts or FIFO watermark infos are
* NOT used here!
*
*/
void L3GD20_initStreamMode(void)
{
// Choose Stream mode:
I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, FIFO_CTRL_REG, FIFO_CTRL_REG_STREAM);
mSleep(10);
// Activate FIFO:
I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG5, CTRL_REG5_FIFO_EN);
mSleep(10);
}
#define DPS_PER_DIGIT 0.00875 // @ +-250 dps (default)
//#define DPS_PER_DIGIT 0.0175 // @ +-500 dps
//#define DPS_PER_DIGIT 0.07 // @ +-2000 dps
#define OFFSET_ZERO 75 // Zero offset (of raw values)
#define D_CORR_FACTOR 1.065 // Corrigation factor
#define GYRO_INTERVAL 50 // Measuring interval [ms]
/**
* This is the L3GD20 gyroscope task.
*
* The task stores the cumulated angle values in
* the global double variables xdg, ydg, zdg.
*
* ATTENTION: Stopwatch 7 is used for the GYRO
* task! Please do not use this
* stopwatch elsewhere in your
* program!
*
* Hint: You must calibrate the gyro carefully
* before using this task!
*
*/
void task_GYRO(void)
{
uint8_t readBuf[6];
if(getStopwatch7() > GYRO_INTERVAL) // GYRO_INTERVAL [ms]
{
I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80));
I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read X-/Y-/Z-axis
// xb = y:
x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L];
// yb = -x:
y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L];
y_axisg *= -1;
// zb = z:
z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L];
normalizeL3GD20();
xg *= DPS_PER_DIGIT; // Calculate dps
yg *= DPS_PER_DIGIT;
zg *= DPS_PER_DIGIT;
xg /= ((double) 1000 / GYRO_INTERVAL); // Angle per interval
yg /= ((double) 1000 / GYRO_INTERVAL);
zg /= ((double) 1000 / GYRO_INTERVAL);
if (abs(x_axisg) > OFFSET_ZERO) // Don't cumulate 0 dps values
xdg += (xg * D_CORR_FACTOR); // Cumulated angle values [°]
if (abs(y_axisg) > OFFSET_ZERO)
ydg += (yg * D_CORR_FACTOR);
if (abs(z_axisg) > OFFSET_ZERO)
zdg += (zg * D_CORR_FACTOR);
setStopwatch7(0);
}
}
/**
* This function resets the cumulated angle
* values [°] stored in the global double
* variables xdg, ydg, zdg by the L3GD20
* gyroscope task.
*
*/
void resetGYRO(void)
{
xdg = 0.0;
ydg = 0.0;
zdg = 0.0;
}
/*****************************************************************************/
/**
* Write a floating point number to the LCD.
*
* Example:
*
* // Write a floating point number to the LCD (no exponent):
* writeDoubleLCD(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5 GYRO!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", "Selftest 5 GYRO");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
clearLCD();
startStopwatch1();
startStopwatch7(); // Used for GYRO task!
// IMPORTANT:
orientation_init(); // Orientation init!
L3GD20_initStreamMode(); // Gyro Stream mode init!
resetGYRO(); // Reset GYRO!
while(true)
{
#ifndef CALIBRATION
task_I2CTWI();
task_GYRO();
#endif
task_I2CTWI();
if(getStopwatch1() > 500) // 500ms
{
#ifdef CALIBRATION
// GYRO calibration part:
readL3GD20(); // Get sensor values
//normalizeL3GD20();
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeIntegerLCD(x_axisg, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8); // line 1 pos 9
writeStringLCD_P("Y");
writeIntegerLCD(y_axisg, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("Z");
writeIntegerLCD(z_axisg, DEC);
writeStringLCD_P(" ");
#else
// Display part:
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeDoubleLCD(xdg, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8); // line 1 pos 9
writeStringLCD_P("Y");
writeDoubleLCD(ydg, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("Z");
writeDoubleLCD(zdg, 6, 1);
writeStringLCD_P(" ");
#endif
setStopwatch1(0);
}
}
return 0;
}
Erklärung
Library für die Messung von Umweltdaten
Zur Messung von Umweltdaten können folgende drei Sensoren genutzt und direkt an die Multi IO Projekt Platine angeschlossen werden:
- Zwei LDR A9060 (Helligkeit)
- HYT-221 (Feuchte, Temperatur)
- BMP085 (Luftdruck, Temperatur)
Bringt man z.B. den HYT-221 und einen LDR A9060 draußen an, dann kann man mit den drei Außenwerten (Feuchte, Temperatur, Helligkeit) und dem innen betriebenen BMP085 und dem zweiten LDR (Luftdruck, Innentemperatur, Innenhelligkeit) schon eine recht gute "Wetterstation" darstellen.
Diese Avr-gcc Library für das Multi IO Projekt Board (= "MultiIO") geht von folgenden Voraussetzungen aus:
- Die RP6v2 M256 WiFi Platine (= "M256") wird für die Ansteuerung der MultiIO benutzt.
- Die M256 ist der I2C-Bus Master.
- Die I2C-Bus Geschwindigkeit beträgt 100 kHz.
- Alle Jumper auf der MultiIO sind in ihrer Standardstellung (1).
- Die MultiIO und die M256 sind mit dem XBUS des RP6-Systems 1:1 verbunden.
- Der Wannenstecker ADC_M256 der MultiIO ist mit dem Wannenstecker ADC_IO1 der M256 1:1 verbunden (2).
Zu (1): Siehe hier! Zu (2): Nur nötig, wenn die Helligkeitssensoren benutzt werden sollen!
3,3V-Stromversorgung
Der I2C-Luftdrucksensor BMP085 wird mit einer Versorgungsspannung von 3,3V und an einem 3,3V-I2C-Bus betrieben. Diese Voraussetzungen sind gegeben, wenn der Sensor an seinen 6-poligen Anschluß-Stecker "Luftdruck" auf der MultiIO angeschlossen wird. Allerdings muss noch die 3,3V-Stromversorgung der MultiIO eingeschaltet werden. Dazu sind zwei zusätzliche Jumper zu setzen, siehe folgende Abbildung:
Pullup-Widerstände
Auf dem I2C-Luftdrucksensormodul mit dem BMP085 Sensor sind Pullup-Widerstände (R1, R2) von je 4,7 kOhm an SCL und SDA vorgesehen. Für die MultiIO Platine sind diese Widerstände nicht erforderlich und müssen deaktiviert werden. Dazu gibt es bei der "BMP085 Breakout" Platine Version v14 einen Jumper SJ1. Dieser Jumper muss auf beiden Seiten aufgetrennt werden, indem das Lötzinn dort entfernt wird. Bei älteren Versionen der Platine ohne den Jumper SJ1 müssen die beiden Pullup-Widerstände ausgelötet werden.
Die Library (Software-Bibliothek) besteht aus drei Teilen:
- Dem Configuration Header -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss.
- Dem Library Header -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library.
- Der Library Source -> Das ist die eigentliche Library.
Zusätzlich wird auch der MultiIO Configuration Header eingebunden. Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss.
Configuration Header
Datei RP6M256_Weather.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples * **************************************************************************** * Example: RP6M256 Weather Library * Author(s): Dirk * **************************************************************************** * Description: * Configuration header file for new Weather library. * * **************************************************************************** */ #ifndef RP6M256_WEATHER_H #define RP6M256_WEATHER_H /*****************************************************************************/ // The following sensors are used: // - Two LDR A9060 brightness sensors (CONRAD No. 145475) // - HYT-221 humidity & temperature module (CONRAD No. 505671) // - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085 // air pressure sensor /*****************************************************************************/ // Define the devices that are connected to the MultiIO Project Board here! // Make a comment of a line belonging to a sensor that is NOT attached to the // MultiIO Board! #define BRIGHTNESS_SENSORS // Brightness sensors (LDRs) are used #define HUMIDITY_TEMP_SENSOR // Humidity & temper. sensor is used #define AIRPRESSURE_SENSOR // Airpressure sensor is used /*****************************************************************************/ // Weather station's data: #define ALTITUDE 156 // Weather station's altitude [m] /*****************************************************************************/ // Brightness sensors (LDRs): #define LDR1_ERROR 0 // LDR1 error (-150 ... +150 lx) #define LDR2_ERROR 0 // LDR2 error (-150 ... +150 lx) /*****************************************************************************/ // Humidity & temperature sensor (HYT-221): #define HYT_RH_ERROR 0.0 // RH error (-2.0 ... +2.0 %) #define HYT_T_ERROR 0.0 // Temp. error (-1.0 ... +1.0 °C) /*****************************************************************************/ // Airpressure sensor (BMP085): #define BMP_P_ERROR 0 // P error (-300 ... +300 Pa) #define BMP_T_ERROR 0 // Temp. error (-20 ... +20 / 10 °C) /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_WeatherLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Header
Datei RP6M256_WeatherLib.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples * **************************************************************************** * Example: RP6M256 Weather Library * Author(s): Dirk * **************************************************************************** * Description: * Header file for new Weather library. * * **************************************************************************** */ #ifndef RP6M256_WEATHERLIB_H #define RP6M256_WEATHERLIB_H /*****************************************************************************/ // The following sensors are used: // - Two LDR A9060 brightness sensors (CONRAD No. 145475) // - HYT-221 humidity & temperature module (CONRAD No. 505671) // - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085 // air pressure sensor /*****************************************************************************/ // Includes: #include "RP6M256Lib.h" #include "RP6I2CmasterTWI.h" #include "RP6M256_Weather.h" #include "RP6M256_MultiIO.h" #include "avr/pgmspace.h" #include "math.h" /*****************************************************************************/ // Defines: #ifndef LEFT #define LEFT 2 #endif #ifndef RIGHT #define RIGHT 3 #endif /*****************************************************************************/ // Brightness sensors (LDRs): #ifdef BRIGHTNESS_SENSORS // Flash Constants: const uint16_t pgm_ldr_adc[17]; const int16_t pgm_ldr_lux[17]; // Variables: extern uint16_t adcldr1_l; // Left LDR1 sensor ADC value extern uint16_t adcldr2_r; // Right LDR2 sensor ADC value extern int16_t luxldr1_l; // Left LDR1 Lux value [lx] extern int16_t luxldr2_r; // Right LDR2 Lux value [lx] uint16_t getBRIGHTNESSSensor(uint8_t side); int16_t calculateBRIGHTNESS(uint8_t side); int16_t measureBRIGHTNESS(uint8_t side); #endif /*****************************************************************************/ // Humidity & temperature sensor (HYT-221): // HYT-221 sensor: #define I2C_MULTIIO_HYT221_ADR 0x50 // Default // Status and data bitmasks: #define HYT221_STATUS_STALE 64 // No new value #define HYT221_STATUS_CMODE 128 // Command mode #define HYT221_RH_H_MASK 63 // RH data high byte (6 bit) // Command mode settings (see "I2C_Address change-HYT1.1.pdf"!): #define CMD_START_CMODE 0x0a // Start command-mode #define CMD_READ_CONFIG_PARAMS 0x1c // Read config. parameters #define CMD_WRITE_CONFIG_PARAMS 0x5c // Write config. parameters #define CMD_FINISH_CMODE 0x80 // Start normal-mode #ifdef HUMIDITY_TEMP_SENSOR extern uint16_t humidity; // Humidity raw value extern uint16_t h_temperature; // Temperature raw value extern double rh; // Humidity value [%] extern double ht; // Temperature value [°C] void HUMIDITY_TEMP_init(void); void HYT221_mr(void); void HYT221_read(void); double HYT221_calculate(void); #define getRHumidity() (rh) #define getHTemperature() (ht) double HYT221_measure(void); #endif /*****************************************************************************/ // Airpressure sensor (BMP085): // BMP085 sensor: #define I2C_MULTIIO_BMP085_ADR 0xee // Default // BMP085 registers: #define BMP085_AC1 0xaa // EEPROM registers (16 bit) #define BMP085_AC2 0xac #define BMP085_AC3 0xae #define BMP085_AC4 0xb0 #define BMP085_AC5 0xb2 #define BMP085_AC6 0xb4 #define BMP085_B1 0xb6 #define BMP085_B2 0xb8 #define BMP085_MB 0xba #define BMP085_MC 0xbc #define BMP085_MD 0xbe #define BMP085_CHIP_ID 0xd0 // Other registers (8 bit) #define BMP085_VERSION 0xd1 #define BMP085_SOFT_RESET 0xe0 #define BMP085_CONTROL 0xf4 // Control register #define BMP085_DATA_MSB 0xf6 // Data registers #define BMP085_DATA_LSB 0xf7 #define BMP085_DATA_XLSB 0xf8 // BMP085 control register settings: #define CONTROL_TEMPERATURE 0x2e // Measure temperature #define CONTROL_PRESSURE_OSRS0 0x34 // Measure pressure mode 0 #define CONTROL_PRESSURE_OSRS1 0x74 // Measure pressure mode 1 #define CONTROL_PRESSURE_OSRS2 0xb4 // Measure pressure mode 2 #define CONTROL_PRESSURE_OSRS3 0xf4 // Measure pressure mode 3 // Airpressure reducing formula constants: #define TEMP_ZERO 273.15 // 273.15 K = 0 °C #ifdef AIRPRESSURE_SENSOR extern int16_t ac1; // EEPROM calibration data extern int16_t ac2; extern int16_t ac3; extern uint16_t ac4; extern uint16_t ac5; extern uint16_t ac6; extern int16_t b1; extern int16_t b2; extern int16_t mb; extern int16_t mc; extern int16_t md; extern int32_t ut; // Temperature raw value extern uint8_t oss; // Mode (0..3) extern int32_t up; // Pressure raw value extern int32_t t; // Temperature [/ 10 °C] extern int32_t p; // Pressure [Pa] extern int32_t p0; // Pressure at sea level [Pa] extern int16_t alt; // Altitude [m] void AIRPRESSURE_init(void); void BMP085_read(void); int32_t BMP085_calculate(void); #define getPressure() (p) #define getTemperature() (t) int32_t BMP085_measure(void); int32_t BMP085_reducePressure(int32_t p, uint16_t alt, int32_t t); int16_t BMP085_getAltitude(int32_t p, int32_t p0, int16_t offset); #endif /*****************************************************************************/ // Weather library initialisation: void weather_init(void); /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_WeatherLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Source
Datei RP6M256_WeatherLib.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 Weather Library
* Author(s): Dirk
* ****************************************************************************
* Description:
*
* This is our new Library that contains basic routines and functions for
* accessing certain sensors connected to the MultiIO Project Board.
*
* ****************************************************************************
*/
/*****************************************************************************/
// The following sensors are used:
// - Two LDR A9060 brightness sensors (CONRAD No. 145475)
// - HYT-221 humidity & temperature module (CONRAD No. 505671)
// - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085
// air pressure sensor
/*****************************************************************************/
// Includes:
#include "RP6M256_WeatherLib.h"
/*****************************************************************************/
// Variables:
uint8_t registerBuf[5];
/*****************************************************************************/
// Brightness sensors (LDRs):
#ifdef BRIGHTNESS_SENSORS
// Flash Constants:
const uint16_t pgm_ldr_adc[] PROGMEM = {
0, 667, 845, 946, 954, 965, 971, 980, 994,
1002, 1010, 1013, 1015, 1017, 1019, 1021, 1023};
const int16_t pgm_ldr_lux[] PROGMEM = {
-32767, 0, 50, 100, 150, 200, 250, 300, 400,
500, 750, 1000, 1500, 2000, 2500, 3000, 32768};
// Variables:
uint16_t adcldr1_l; // Left LDR1 sensor ADC value
uint16_t adcldr2_r; // Right LDR2 sensor ADC value
/**
* This function reads and returns the ADC value
* of the left or right brightness (LDR) sensor.
* The value is also stored in adcldr1_l or
* adcldr2_r.
*
* Input: side -> 2 (LEFT) = Left LDR1 sensor
* 3 (RIGHT) = Right LDR2 sensor
*
*/
uint16_t getBRIGHTNESSSensor(uint8_t side)
{
if (side == LEFT) {
adcldr1_l = readADC(ADC_MULTIIO_LDR1);
return adcldr1_l;
}
if (side == RIGHT) {
adcldr2_r = readADC(ADC_MULTIIO_LDR2);
return adcldr2_r;
}
return 0;
}
int16_t luxldr1_l; // Left LDR1 Lux value [lx]
int16_t luxldr2_r; // Right LDR2 Lux value [lx]
/**
* Calculates and returns the left or right LDR
* sensor light intensity value [lx] by using
* the data read from the LDR sensor with the
* function getBRIGHTNESSSensor().
*
* Input: side -> 2 (LEFT) = Left LDR1 sensor
* 3 (RIGHT) = Right LDR2 sensor
*
* Hint: - The lookup table pgm_ldr_adc and
* pgm_ldr_lux is adapted to the
* PerkinElmer LDR A9060_A9013 sensor
* ONLY (R41, R42 = 68 kOhm)!
*
*/
int16_t calculateBRIGHTNESS(uint8_t side)
{
uint8_t i;
int16_t lux_h, lux_l, vside;
uint16_t adc_h, adc_l;
double dvside;
if (side == LEFT) {
for (i = 1; i < 17; i++) {
adc_h = pgm_read_word(&pgm_ldr_adc[i]);
if (adcldr1_l <= adc_h) {
break;
}
}
adc_l = pgm_read_word(&pgm_ldr_adc[i - 1]);
lux_h = pgm_read_word(&pgm_ldr_lux[i]);
lux_l = pgm_read_word(&pgm_ldr_lux[i - 1]);
dvside = ((double) adcldr1_l - adc_l) / ((double) adc_h - adc_l)
* ((double) lux_h - lux_l) + (double) lux_l;
vside = dvside;
vside += LDR1_ERROR;
return vside;
}
if (side == RIGHT) {
for (i = 1; i < 17; i++) {
adc_h = pgm_read_word(&pgm_ldr_adc[i]);
if (adcldr2_r <= adc_h) {
break;
}
}
adc_l = pgm_read_word(&pgm_ldr_adc[i - 1]);
lux_h = pgm_read_word(&pgm_ldr_lux[i]);
lux_l = pgm_read_word(&pgm_ldr_lux[i - 1]);
dvside = ((double) adcldr2_r - adc_l) / ((double) adc_h - adc_l)
* ((double) lux_h - lux_l) + (double) lux_l;
vside = dvside;
vside += LDR2_ERROR;
return vside;
}
return 0;
}
/**
* Measures and returns the left or right LDR
* sensor light intensity value [lx].
* The ADC value of the LDR sensor is also
* stored in adcldr1_l or adcldr2_r.
*
* Input: side -> 2 (LEFT) = Left LDR1 sensor
* 3 (RIGHT) = Right LDR2 sensor
*
*/
int16_t measureBRIGHTNESS(uint8_t side)
{
uint16_t tmp;
tmp = getBRIGHTNESSSensor(side);
return (calculateBRIGHTNESS(side)); // Result in [lx]
}
#endif
/*****************************************************************************/
// Humidity & temperature sensor (HYT-221):
#ifdef HUMIDITY_TEMP_SENSOR
/**
* Call this once before using the humidity
* & temperature sensor.
*
*/
void HUMIDITY_TEMP_init(void)
{
// Nothing to do here in this software version ...
}
/**
* Measuring request (MR) command. This function
* starts a measurement. The result is NOT read
* by this function (see HYT221_read()!).
*
*/
void HYT221_mr(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_HYT221_ADR, 0);
}
uint16_t humidity; // Humidity raw value
uint16_t h_temperature; // Temperature raw value
/**
* Data fetch (DF) command. This function reads
* the humidity and temperature raw values from
* the humidity & temperatur sensor and stores
* them in the global variables humidity and
* h_temperature.
*
* Hints: - Each measurement has to be started
* with the function HYT_mr() before!
* - If the Stale bit (bit 14 of the RH
* data high byte) is set, the data are
* NOT new data and have already been
* read. The Stale bit is not used in
* this function.
* - You have to make a pause between
* starting the measurement (HYT_mr()!)
* and reading the result with this
* function OR you have to poll the
* Stale bit (0 = new data).
*
*/
void HYT221_read(void)
{
I2CTWI_readBytes(I2C_MULTIIO_HYT221_ADR, registerBuf, 4);
humidity = ((registerBuf[0] & HYT221_RH_H_MASK) << 8) | registerBuf[1];
h_temperature = ((registerBuf[2] << 8) | registerBuf[3]) >> 2;
}
double rh; // Humidity value [%]
double ht; // Temperature value [°C]
/**
* This function calculates the humidity and
* temperature values by using the sensor raw
* values humidity and h_temperature (see
* function HYT_read()!). The humidity value
* will be returned and stored in the global
* double variable rh. The temperature [°C] is
* stored in the global double variable ht.
*
* Output: Humidity [%]
*
*/
double HYT221_calculate(void)
{
rh = (double) humidity * 100.0 / 16383.0;
rh += HYT_RH_ERROR;
ht = (double) h_temperature * 165.0 / 16383.0 - 40.0;
ht += HYT_T_ERROR;
return rh;
}
/**
* Measures and returns the humidity [%] value.
* The temperature [°C] is stored in the global
* double variable ht.
* The humidity and temperature raw values are
* also stored in the global variables humidity
* and h_temperature.
* This function is BLOCKING for about 100 ms!
* If you don't want a blocking measurement, you
* have to write your own function with a non
* blocking pause between starting measurement
* and reading the result.
*
* Output: Humidity [%]
*
*/
double HYT221_measure(void)
{
HYT221_mr();
mSleep(100);
HYT221_read();
return (HYT221_calculate()); // Result in [%]
}
#endif
/*****************************************************************************/
// Airpressure sensor (BMP085):
#ifdef AIRPRESSURE_SENSOR
int16_t ac1; // EEPROM calibration data
int16_t ac2;
int16_t ac3;
uint16_t ac4;
uint16_t ac5;
uint16_t ac6;
int16_t b1;
int16_t b2;
int16_t mb;
int16_t mc;
int16_t md;
int32_t ut; // Temperature raw value
uint8_t oss = 1; // Default: standard mode
uint8_t control_pressure_osrs; // Control register mode setting
int32_t up; // Pressure raw value
int32_t t; // Temperature [/ 10 °C]
int32_t p; // Pressure [Pa]
int32_t p0; // Pressure at sea level [Pa]
int16_t alt; // Altitude [m]
/**
* Call this once before using the airpressure
* sensor.
*
*/
void AIRPRESSURE_init(void)
{
// Mode:
if (oss > 3) oss = 1;
control_pressure_osrs = (oss << 6) | CONTROL_PRESSURE_OSRS0;
// Calibration (EEPROM) data:
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC1);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ac1 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC2);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ac2 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC3);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ac3 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC4);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ac4 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC5);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ac5 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_AC6);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ac6 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_B1);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
b1 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_B2);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
b2 = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_MB);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
mb = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_MC);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
mc = (registerBuf[0] << 8) | registerBuf[1];
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_MD);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
md = (registerBuf[0] << 8) | registerBuf[1];
}
/**
* This function reads the pressure and
* temperature raw values from the airpressure
* sensor and stores them in the global long
* variables ut and up.
*
*/
void BMP085_read(void)
{
// Temperature:
I2CTWI_transmit2Bytes(I2C_MULTIIO_BMP085_ADR, BMP085_CONTROL, CONTROL_TEMPERATURE);
mSleep(10);
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_DATA_MSB);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 2);
ut = (registerBuf[0] << 8) | registerBuf[1];
// Pressure:
control_pressure_osrs = (oss << 6) | CONTROL_PRESSURE_OSRS0;
I2CTWI_transmit2Bytes(I2C_MULTIIO_BMP085_ADR, BMP085_CONTROL, control_pressure_osrs);
mSleep(30);
I2CTWI_transmitByte(I2C_MULTIIO_BMP085_ADR, BMP085_DATA_MSB);
I2CTWI_readBytes(I2C_MULTIIO_BMP085_ADR, registerBuf, 3);
up = (((uint32_t) registerBuf[0] << 16)
| ((uint32_t) registerBuf[1] << 8)
| ((uint32_t) registerBuf[2])) >> (8 - oss);
}
/**
* This function calculates the pressure and
* temperature values by using the sensor raw
* values ut, up (see function BMP085_read()!)
* and the EEPROM calibration data values ac1,
* ac2, ac3, ac4, ac5, ac6, b1, b2, mb, mc, md
* (see function AIRPRESSURE_init()!). The
* airpressure [Pa] value will be returned and
* stored in the global long variable p. The
* temperature [°C] is stored in the global long
* variable t.
*
* Output: Airpressure [Pa]
*
*/
int32_t BMP085_calculate(void)
{
int32_t x1, x2, b5, b6, x3, b3;
uint32_t b4, b7;
// Temperature:
x1 = ((int32_t) ut - ac6) * ac5 >> 15;
x2 = ((int32_t) mc << 11) / (x1 + md);
b5 = x1 + x2;
t = (b5 + 8) >> 4;
t += BMP_T_ERROR;
// Pressure:
b6 = b5 - 4000;
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x2 = ac2 * b6 >> 11;
x3 = x1 + x2;
b3 = ((((int32_t) ac1 * 4 + x3) << oss) + 2) >> 2;
x1 = ac3 * b6 >> 13;
x2 = (b1 * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
b4 = ac4 * (uint32_t) (x3 + 32768) >> 15;
b7 = ((uint32_t) up - b3) * (50000 >> oss);
p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1;
x1 = (p >> 8) * (p >> 8);
x1 = (x1 * 3038) >> 16;
x2 = (-7357 * p) >> 16;
p = p + ((x1 + x2 + 3791) >> 4);
p += BMP_P_ERROR;
return p;
}
/**
* Measures and returns the airpressure [Pa]
* value.
* The temperature [°C] is stored in the global
* long variable t.
* The pressure and temperature raw values are
* also stored in the global long variables ut
* and up.
*
* Output: Airpressure [Pa]
*
*/
int32_t BMP085_measure(void)
{
BMP085_read(); // Read data
return (BMP085_calculate()); // Result in [Pa]
}
/**
* Reduces the absolute airpressure [Pa] value
* and returns the airpressure at sea level
* value [Pa]. The airpressure at sea level is
* also stored in the global long variable p0.
*
* Input: p -> Airpressure [Pa]
* alt -> Altitude [m]
* t -> Temperature [/ 10 °C]
*
* Output: Airpressure at sea level [Pa]
*
*/
int32_t BMP085_reducePressure(int32_t p, uint16_t alt, int32_t t)
{
double dp0;
double dt = t / 10;
dp0 = (double) p * (pow ((TEMP_ZERO + dt)
/ (TEMP_ZERO + dt + 0.0065 * alt), -5.255));
p0 = (int32_t) dp0;
return p0; // Result in [Pa]
}
/**
* Calculates and returns the altitude [m] by
* using the absolute airpressure [Pa] value.
*
* Input: p -> Airpressure [Pa]
* p0 -> Airpressure at sea level
* [Pa]
* offset -> Altitude offset [m]
*
* Output: Altitude [m]
*
* Hint: If p0 (airpressure at sea level) is
* unknown, set p0 to zero and adjust the
* altitude result with offset!
*
*/
int16_t BMP085_getAltitude(int32_t p, int32_t p0, int16_t offset)
{
float palt;
int16_t alt;
if ((p0 == 0) || (p0 < p)) p0 = 101325;
palt = pow (((double) p / p0), (1.0 / 5.255));
palt = 1 - palt;
palt = palt / 0.0000225577;
alt = (int16_t) palt;
alt += offset;
return alt; // Result in [m]
}
#endif
/*****************************************************************************/
/*****************************************************************************/
// Weather library initialisation:
/**
* Call this once before using the airpressure
* sensor.
*
*/
void weather_init(void)
{
#ifdef HUMIDITY_TEMP_SENSOR
HUMIDITY_TEMP_init();
#endif
#ifdef AIRPRESSURE_SENSOR
AIRPRESSURE_init();
#endif
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.0 (initial release) 09.07.2013 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6M256_MultiIO_06 ... SRC += RP6M256_WeatherLib.c ...
Datei RP6M256_MultiIO_06.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a sixth test for the MultiIO Project Board.
* It tests the "Weather Sensors" (for brightness, humidity, temperature
* and air pressure).
*
* The following sensors are used:
* - Two LDR A9060 brightness sensors (CONRAD No. 145475)
* - HYT-221 humidity & temperature module (CONRAD No. 505671)
* - Barometric pressure sensor (Watterott No. SEN-11282) with the BMP085
* air pressure sensor
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* You should also connect to it via WIFI.
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h" // The RP6 M256 Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Weather library":
// (This is the library for accessing certain sensors connected to the
// MultiIO Project Board!)
#include "RP6M256_WeatherLib.h"
/*****************************************************************************/
// Defines:
//#define SHOW_BMP085_EEPROM_DATA
#define ALT_OFFSET 5
/*****************************************************************************/
/**
* Write a floating point number to the WIFI.
*
* Example:
*
* // Write a floating point number to the WIFI (no exponent):
* writeDouble_WIFI(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString_WIFI(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
writeInteger_WIFI(errorState, HEX);
writeChar_WIFI('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6M256(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 6!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
mSleep(2500);
showScreenLCD("RP6M256 Multi IO", " Selftest 6");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
#ifdef BRIGHTNESS_SENSORS
int16_t lux_l, lux_r;
#endif
#ifdef HUMIDITY_TEMP_SENSOR
double rhum;
#endif
#ifdef AIRPRESSURE_SENSOR
int32_t airpressure;
int32_t airpressure0;
int16_t calcalt;
#endif
startStopwatch1();
// IMPORTANT:
weather_init(); // Weather init!
while(true)
{
if(getStopwatch1() > 1000) // 1s
{
#ifdef BRIGHTNESS_SENSORS
// Brightness sensors test:
clearLCD();
lux_l = measureBRIGHTNESS(LEFT); // Read left LDR1
setCursorPosLCD(0, 0);
writeStringLCD("ADC ");
writeIntegerLCD(adcldr1_l, DEC);
writeStringLCD(" ");
lux_r = measureBRIGHTNESS(RIGHT); // Read right LDR2
setCursorPosLCD(0, 10);
writeIntegerLCD(adcldr2_r, DEC);
writeStringLCD(" ");
setCursorPosLCD(1, 0);
writeStringLCD("LUX ");
writeIntegerLCD(lux_l, DEC);
writeStringLCD(" ");
setCursorPosLCD(1, 10);
writeIntegerLCD(lux_r, DEC);
writeStringLCD(" ");
#endif
#ifdef HUMIDITY_TEMP_SENSOR
// Humidity & temperature sensor test:
rhum = HYT221_measure(); // Read humidity, temperature
writeString_WIFI("\nHYT-221 SENSOR ->");
writeString_WIFI("\n- Humidity: ");
writeDouble_WIFI(rhum, 5, 1);
writeString_WIFI(" %\n- Temperature: ");
writeDouble_WIFI(ht, 5, 1);
writeString_WIFI(" °C\n");
#endif
#ifdef AIRPRESSURE_SENSOR
// Airpressure sensor test:
writeString_WIFI("\nBMP085 SENSOR ->");
#ifdef SHOW_BMP085_EEPROM_DATA
writeString_WIFI("\n- Calibration data:");
writeString_WIFI("\n AC1: ");
writeInteger_WIFI(ac1, DEC);
writeString_WIFI("\n AC2: ");
writeInteger_WIFI(ac2, DEC);
writeString_WIFI("\n AC3: ");
writeInteger_WIFI(ac3, DEC);
writeString_WIFI("\n AC4: ");
write32BitInteger_WIFI(ac4, DEC);
writeString_WIFI("\n AC5: ");
write32BitInteger_WIFI(ac5, DEC);
writeString_WIFI("\n AC6: ");
write32BitInteger_WIFI(ac6, DEC);
writeString_WIFI("\n B1: ");
writeInteger_WIFI(b1, DEC);
writeString_WIFI("\n B2: ");
writeInteger_WIFI(b2, DEC);
writeString_WIFI("\n MB: ");
writeInteger_WIFI(mb, DEC);
writeString_WIFI("\n MC: ");
writeInteger_WIFI(mc, DEC);
writeString_WIFI("\n MD: ");
writeInteger_WIFI(md, DEC);
writeChar_WIFI('\n');
#endif
airpressure = BMP085_measure(); // Read pressure, temperature
writeString_WIFI("\n- Air pressure [");
writeIntegerLength_WIFI(ALTITUDE, DEC, 4);
writeString_WIFI(" m]: ");
write32BitInteger_WIFI(airpressure, DEC);
// Reduce airpressure:
airpressure0 = BMP085_reducePressure(airpressure, ALTITUDE, t);
writeString_WIFI(" Pa\n- Air pressure [0000 m]: ");
write32BitInteger_WIFI(airpressure0, DEC);
writeString_WIFI(" Pa\n- Temperature [");
writeIntegerLength_WIFI(ALTITUDE, DEC, 4);
writeString_WIFI(" m]: ");
write32BitInteger_WIFI(t, DEC);
// Calculate altitude:
calcalt = BMP085_getAltitude(airpressure, airpressure0, ALT_OFFSET);
writeString_WIFI(" / 10 °C\n- Calculated altitude : ");
writeIntegerLength_WIFI(calcalt, DEC, 4);
writeString_WIFI(" m\n");
#endif
setStopwatch1(0);
}
task_I2CTWI();
}
return 0;
}
Erklärung
DCF77-Library
Die "Funkuhr-Zeit" kann empfangen und ausgewertet werden, wenn ein DCF77-Empfänger (z.B. CONRAD 641138) mit dem dafür vorgesehenen Anschluß "DCF77" auf der Multi IO Platine verbunden ist.
Die RP6 M256 WIFI DCF77 Library findet ihr HIER!
Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6M256_DCFLib.c an.
Die folgenden Zeilen am Anfang der Datei im Abschnitt "Defines":
// RP6 M256 WIFI DCF Receiver Connection: #define DCF_PORT PORTJ #define DCF_DDR DDRJ #define DCF_PIN PINJ #define DCF_IN INT1_PI12 // PINJ3 XBUS Pin 8
... müssen so geändert werden:
// RP6 M256 WIFI DCF Receiver Connection: #define DCF_PORT PORTE #define DCF_DDR DDRE #define DCF_PIN PINE #define DCF_IN IO_PE7_ICP3_I7 // PINE7 IO_PWM/T2/T3 Pin 1
RP6 CCPRO M128
Die RP6-CCPRO-M128 (= "M128") kann über den I2C-Bus alle I2C-Sensoren und I2C-Aktoren ansteuern und hat mit ihren freien Ressourcen (Portpins) gute Voraussetzungen, um die weiteren Funktionen der Multi IO Platine zu nutzen. Dazu wird sie (wenn alle Funktionen gleichzeitig genutzt werden sollen) mit zwei Steckverbindungen angeschlossen:
- IO-Mxxx <-> I/O
- ADC-Mxxx <-> ADC
Dadurch ergeben sich umfangreiche Möglichkeiten, die Sensoren der Multi IO Platine auszulesen und Aktoren zu schalten. Die folgenden 2 Tabellen zeigen die Verbindungen:
Stecker IO-Mxxx:
| Stecker-Pin | M128-Port | M128-Funktion | Port-Bit | Multi IO-Funktion | I/O |
| 1 | PB5 | OC1A | 13 | DCF77 | I |
| 2 | PD7 | T2 | 31 | LFS_PWR | O |
| 3 | PB6 | OC1B | 14 | SHARPS_PWR | O |
| 4 | PD6 | T1 | 30 | SNAKE_SWITCH | O |
| 5 | PE3 ** | OC3A/AIN1 | 35 | BUZZER | O |
| 6 | PD4 | ICP1 | 28 | (TX *) | |
| 7 | PD3 | TXD1/INT3 | 27 | SNAKE_KEY / TX | I/O |
| 8 | PD2 | RXD1/INT2 | 26 | RX | I |
| 9 | GND | ||||
| 10 | VDD |
Zu *) TX nicht an Pin 6 des I/O Steckers der M128 vorhanden!
Zu **) Auf der M128 ist hier ebenfalls der Piezo-Signalgeber (SND) angeschlossen!
Stecker ADC-Mxxx:
| Stecker-Pin | M128-Port | M128-Funktion | Port-Bit | Multi IO-Funktion | I/O |
| 1 | PF1 | ADC1 | 41 | TOUCH / LFS_L | I |
| 2 | PF7 | ADC7 | 47 | 3V3 alt.2 | I |
| 3 | PF3 | ADC3 | 43 | 3V3 / LFS_M | I |
| 4 | PF0 | ADC0 | 40 | BUMPER_L | I/O * |
| 5 | PF5 | ADC5 | 45 | LFS_R | I |
| 6 | PF2 | ADC2 | 42 | n.c. | |
| 7 | PF6 | ADC6 | 46 | BUTTONS | I |
| 8 | PF4 | ADC4 | 44 | BUMPER_R | I/O * |
| 9 | GND | ||||
| 10 | VDD |
Zu *) Eingang für Bumper-Taster, Ausgang für Bumper-LED!
Multi IO Projekt Library
Die Multi IO Library für die M128 hat bei mir nicht die erste Priorität. Wenn jemand die Multi IO Platine unbedingt mit der M128 betreiben will, bin ich mit Tips für das Umschreiben der Library auf CompactC gern dabei!
Abweichende IO-Jumperstellung für die M128
Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!
Zwei Signalgeber am selben Portpin
WICHTIGER HINWEIS: Es ist nur möglich, den Signalgeber (Buzzer SG2) auf der Multi IO Platine zu nutzen, wenn der Signalgeber (SND) auf der M128 deaktiviert wurde (Jumper "EN_SND" auf der M128 abziehen)! Wenn beide Signalgeber GLEICHZEITIG mit dem Portpin PE3 der M128 verbunden sind, kann dieser evtl. beschädigt werden!!! Soll der Buzzer auf der MultiIO NICHT genutzt werden, muss der Jumper "Buz" auf Pin 5 des Jumper-Blocks "J_IO" abgezogen werden. Siehe folgende Abbildung:
Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!
I2C-Adresse des TCN75A
WICHTIGER HINWEIS: Wird die RP6 CCPRO M128 zur Ansteuerung der MultiIO benutzt ODER ist eine RP6 CCPRO M128 gemeinsam mit der MultiIO mit dem XBUS des RP6-Systems verbunden, dann befinden sich ZWEI TCN75A (auf der MultiIO UND der M128) am selben I2C-Bus! Beide Sensoren sind standardmäßig auf dieselbe I2C-Adresse (0x90) eingestellt.
Einer der beiden TCN75A muss deshalb auf eine andere I2C-Adresse umgestellt werden (im Bild oben ist mit "J_TCN_ADR" eine neue Standard-I2C-Adresse 0x92 für den TCN75A auf der MultiIO eingestellt)!!!
Alternativ: Der TCN75A (U26) auf der MultiIO wird deaktiviert ("J_TCN" abgezogen)! Siehe nachfolgende Abbildung:
Analoge Sensoren auf dem Bumper Board
Mit dem Wahl-Jumper-Block (s.o. Abbildung "Abweichende IO-Jumperstellung für die M128"!) sind die Signalleitungen ("SigL" und "SigR") des Bumper Boards mit "SCL" und "SDA", also mit dem I2C-Bus verbunden (z.B. für SRF02). Will man stattdessen analoge Sensoren (SHARP, Radar) auf dem Bumper Board nutzen (mit der Linienfolger und Bumper Board Library für die M128), dann kann man mit dem Wahl-Jumper-Block leider keine analogen (ADC-) Eingänge dafür auswählen, weil "PF4" und "PF3" nur auf der M256 als ADC-Eingänge verfügbar sind.
Eine mögliche Lösung ist die Nutzung der beiden freien ADC-Kanäle 7 und 2 der M128 für die analogen Sensoren auf dem Bumper Board. Die folgende Abbildung zeigt die dafür nötigen Verbindungen:
Linienfolger Sensoren (LFS) anschliessen
Will man die Linienfolger Sensoren (LFS) anstelle der 3,3V-Messung und des Touch Sensors nutzen (mit der Linienfolger und Bumper Board Library für die M128), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block:
Hinweis: Die Jumper sind orange eingezeichnet!
Library
Datei RP6MultiIOCClib.cc:
Die Library ist nicht vollständig! Aktuell sind dies nur die Definitionen, die für die MultiIO benötigt werden!
/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> CCPRO M128 * ---------------------------------------------------------------------------- * ---------------------------- [c]2013 - Dirk -------------------------------- * **************************************************************************** * File: RP6MultiIOCClib.cc * Version: 1.0 - CompactC * Target: RP6 CCPRO M128 - C-Control PRO M128 @14.7456MHz * mit optionalem LC-Display 16x2 Zeichen (CONRAD 190911) * [oder Backlight Display 16x2 Zeichen (CONRAD 191621)] * und mit der MultiIO Projekt Platine ... * Author(s): Dirk * **************************************************************************** * Beschreibung: * Dies ist meine RP6 CCPRO M128 MultiIO Bibliothek. Sie enthält Funktionen, * um die MultiIO Projekt Platine mit der RP6 CCPRO M128 anzusteuern. * Die Dokumentation zum Projekt ist hier zu finden: * ---> http://www.rn-wissen.de/index.php/RP6_Multi_IO_Projekt <--- * ---> http://www.rn-wissen.de/index.php/RP6_Multi_IO_Projekt_-_Software <--- * * ... * * **************************************************************************** * CHANGELOG FINDEN SIE AM ENDE DIESER DATEI! * **************************************************************************** */ // Alles nur EINMAL einbinden! #ifndef __RP6MULTIIOCCLIB__ #define __RP6MULTIIOCCLIB__ /*****************************************************************************/ // Auf der MultiIO montierte Komponenten: // - I2C Spannung & Strom Sensor (LTC2990) // - I2C Echtzeituhr (RTC DS1307Z) // - I2C Temperatur Sensor (TCN75A) // - I2C Servo Controller (PCA9685) // - I2C EEPROM (24LCXXX) // - 3V3 Spannung Sensor // - Touch Sensor (mit NE555) // - Buttons // - LEDs // - Buzzer /*****************************************************************************/ // Konfiguration: // I2C Spannung & Strom Sensor (LTC2990): #define I2C_MULTIIO_VCS_ADR 0x98 // ADR1/0 = 0/0 //#define I2C_MULTIIO_VCS_ADR 0x9a // ADR1/0 = 0/1 //#define I2C_MULTIIO_VCS_ADR 0x9c // ADR1/0 = 1/0 //#define I2C_MULTIIO_VCS_ADR 0x9e // ADR1/0 = 1/1 //#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address // (Abgleich von Shunt Widerstand, Strom und Spannung!) #define SHUNT_R 0.051 // R10 [Ohm] #define C_ADJUST 2.0 // Strom Abgleich-Faktor #define C_OFFSET 0.0 // Strom Offset [mA] #define VBAT_ADJUST 3.2 // (R13 + R14) / R14 #define VSERVO_ADJUST 2.5 // (R32 + R22) / R22 /*****************************************************************************/ // I2C Echtzeituhr (RTC DS1307Z): #define I2C_MULTIIO_RTC_ADR 0xD0 // Default /*****************************************************************************/ // I2C Temperatur Sensor (TCN75A): // (A1 immer 0!) //#define I2C_MULTIIO_TEMP_ADR 0x90 // A2/0 = 0/0 #define I2C_MULTIIO_TEMP_ADR 0x92 // A2/0 = 0/1 //#define I2C_MULTIIO_TEMP_ADR 0x98 // A2/0 = 1/0 //#define I2C_MULTIIO_TEMP_ADR 0x9a // A2/0 = 1/1 /*****************************************************************************/ // I2C Servo Controller (PCA9685): // (A5, A4, A3, A2 immer 0!) #define I2C_MULTIIO_SERVO_ADR 0x80 // A1/0 = 0/0 //#define I2C_MULTIIO_SERVO_ADR 0x82 // A1/0 = 0/1 //#define I2C_MULTIIO_SERVO_ADR 0x84 // A1/0 = 1/0 //#define I2C_MULTIIO_SERVO_ADR 0x86 // A1/0 = 1/1 //#define I2C_MULTIIO_SERVO_ADR 0xe0 // ALLCALLADR // (Servo Power ist verbunden mit LED8 des PCA9685!) #define CHSERVOPWR 9 // Konstanten für linker Servo Anschlag (LT), rechter Servo Anschlag (RT), // mittlere Servo Position (MP): // (Hinweise: - Servo Impuls Länge [ms] = Servo Positionswert / 204.8 // (Formel nur gültig für eine PWM von 50 Hz!) // - Min. Servo Impuls (0,7 ms) = Servo Positionswert 143 // - Mid. Servo Impuls (1,5 ms) = Servo Positionswert 307 // - Max. Servo Impuls (2,3 ms) = Servo Positionswert 471 // - !!! Keine Positionswerte < 143 oder > 471 benutzen!!!) #define SERVO1_LT 205 // Servo Impuls ~1ms #define SERVO1_RT 410 // Servo Impuls ~2ms #define SERVO1_MP ((SERVO1_RT - SERVO1_LT) / 2 + SERVO1_LT) #define SERVO2_LT 205 #define SERVO2_RT 410 #define SERVO2_MP ((SERVO2_RT - SERVO2_LT) / 2 + SERVO2_LT) #define SERVO3_LT 205 #define SERVO3_RT 410 #define SERVO3_MP ((SERVO3_RT - SERVO3_LT) / 2 + SERVO3_LT) #define SERVO4_LT 205 #define SERVO4_RT 410 #define SERVO4_MP ((SERVO4_RT - SERVO4_LT) / 2 + SERVO4_LT) #define SERVO5_LT 205 #define SERVO5_RT 410 #define SERVO5_MP ((SERVO5_RT - SERVO5_LT) / 2 + SERVO5_LT) #define SERVO6_LT 205 #define SERVO6_RT 410 #define SERVO6_MP ((SERVO6_RT - SERVO6_LT) / 2 + SERVO6_LT) #define SERVO7_LT 205 #define SERVO7_RT 410 #define SERVO7_MP ((SERVO7_RT - SERVO7_LT) / 2 + SERVO7_LT) #define SERVO8_LT 205 #define SERVO8_RT 410 #define SERVO8_MP ((SERVO8_RT - SERVO8_LT) / 2 + SERVO8_LT) /*****************************************************************************/ // I2C EEPROM (24LCXXX): // (A2=1 nicht mit 24LC1024-P nutzbar!) #define I2C_MULTIIO_EEPROM_ADR 0xA0 // A2/1/0 = 0/0/0 //#define I2C_MULTIIO_EEPROM_ADR 0xA2 // A2/1/0 = 0/0/1 //#define I2C_MULTIIO_EEPROM_ADR 0xA4 // A2/1/0 = 0/1/0 //#define I2C_MULTIIO_EEPROM_ADR 0xA6 // A2/1/0 = 0/1/1 //#define I2C_MULTIIO_EEPROM_ADR 0xA8 // A2/1/0 = 1/0/0 //#define I2C_MULTIIO_EEPROM_ADR 0xAA // A2/1/0 = 1/0/1 //#define I2C_MULTIIO_EEPROM_ADR 0xAC // A2/1/0 = 1/1/0 //#define I2C_MULTIIO_EEPROM_ADR 0xAE // A2/1/0 = 1/1/1 // Definiere die I2C-EEPROM Speicherkapazität [kbit] hier: #define I2C_EEPROM_KBIT 32 // 24LC32-P <== Standard //#define I2C_EEPROM_KBIT 64 // 24LC64-P //#define I2C_EEPROM_KBIT 128 // 24LC128-P //#define I2C_EEPROM_KBIT 256 // 24LC256-P //#define I2C_EEPROM_KBIT 512 // 24LC512-P //#define I2C_EEPROM_KBIT 1024 // 24LC1024-P // Definiere die I2C-EEPROM Seitengröße [byte] hier: // ACHTUNG: Die Seitengröße muss zum oben definierten EEPROM Typ passen! #define I2C_EEPROM_PAGESIZE 32 // EEPROM 32 or 64 kbit //#define I2C_EEPROM_PAGESIZE 64 // EEPROM 128 or 256 kbit //#define I2C_EEPROM_PAGESIZE 128 // EEPROM 512 kbit //#define I2C_EEPROM_PAGESIZE 256 // EEPROM 1024 kbit /*****************************************************************************/ // 3V3 Spannung Sensor: #define ADC_MULTIIO_3V3 3 // PortF.3 ADC-Mxxx: ADC //#define ADC_MULTIIO_3V3 7 // PortF.7 ADC-Mxxx: ADC /*****************************************************************************/ // Touch Sensor (mit NE555): #define ADC_MULTIIO_TOUCH 1 // PortF.1 ADC-Mxxx: ADC #define ADCVAL_NOTOUCH 15 #define ADCVAL_TOUCH 1020 #define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) /*****************************************************************************/ // Buttons: #define ADC_MULTIIO_BUTTONS 6 // PortF.6 ADC-Mxxx: ADC #define ADCVAL_BUTTON1 14 #define ADCVAL_BUTTON2 577 #define ADCVAL_BUTTON3 736 #define ADCVAL_BUTTON4 812 #define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) #define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) #define ADCVAL_LIMIT34 ((ADCVAL_BUTTON4 - ADCVAL_BUTTON3) / 2 + ADCVAL_BUTTON3) #define ADCVAL_LIMIT40 ((1023 - ADCVAL_BUTTON4) / 2 + ADCVAL_BUTTON4) /*****************************************************************************/ // LEDs: // (Status LED1..LED4 sind verbunden mit LED15..LED12 des PCA9685!) #define CHLED1 16 #define CHLED2 15 #define CHLED3 14 #define CHLED4 13 /*****************************************************************************/ // Buzzer: // (Der IO-Mxxx Stecker ist verbunden mit dem M128 I/O Stecker!) #define IO_MULTIIO_BUZZER_PORT 35 // PortE.3 IO-Mxxx: I/O /*****************************************************************************/ // Andere ADC Kanal Definitionen: // (Abhängig von Jumper-Stellungen auf der MultiIO Projekt Platine!) // (Der ADC-Mxxx Stecker ist verbunden mit dem M128 ADC Stecker!) #define ADC_MULTIIO_LFS_L 1 // PortF.1 ADC-Mxxx: ADC #define ADC_MULTIIO_LFS_M 3 // PortF.3 ADC-Mxxx: ADC #define ADC_MULTIIO_LFS_R 5 // PortF.5 ADC-Mxxx: ADC #define ADC_MULTIIO_BUMPER_L 0 // PortF.0 ADC-Mxxx: ADC #define ADC_MULTIIO_BUMPER_R 4 // PortF.4 ADC-Mxxx: ADC #define ADC_MULTIIO_SHARP_L 7 // PortF.7 ADC-Mxxx: ADC #define ADC_MULTIIO_SHARP_R 2 // PortF.2 ADC-Mxxx: ADC // Andere IO Portpin Definitionen: // (Der IO-Mxxx Stecker ist verbunden mit dem M128 I/O Stecker!) #define IO_MULTIIO_LFS_PWR_PORT 31 // PortD.7 IO-Mxxx: I/O #define IO_MULTIIO_SHARPS_PWR_PORT 14 // PortB.6 IO-Mxxx: I/O #define IO_MULTIIO_SNAKE_SWITCH_PORT 30 // PortD.6 IO-Mxxx: I/O #define IO_MULTIIO_SNAKE_KEY_PORT 27 // PortD.3 IO-Mxxx: I/O #define IO_MULTIIO_DCF77_PORT 13 // PortB.5 IO-Mxxx: I/O /*****************************************************************************/ // Ende Definitionen
Erklärung
Demo
Erklärung
DCF77-Library
Die "Funkuhr-Zeit" kann empfangen und ausgewertet werden, wenn ein DCF77-Empfänger (z.B. CONRAD 641138) mit dem dafür vorgesehenen Anschluß "DCF77" auf der Multi IO Platine verbunden ist.
Die RP6 CCPRO M128 DCF77 Library in CompactC findet ihr HIER (Teil 1) und HIER (Teil 2)!
Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6DCFCClib.cc an.
Die folgenden Zeilen am Anfang der Datei im Abschnitt "Defines":
// RP6 CCPRO M128 DCF Empfänger Anschluß: #define DCF_IN 37 // DCF Eingangs Signale an PortE.5 (RP6: INT1)
... müssen so geändert werden:
// RP6 CCPRO M128 DCF Empfänger Anschluß: #define DCF_IN 13 // DCF Eingangs Signale an PortB.5 (M128: OC1A)
Weitere Software
Weitere Software einschließlich Libraries für den Betrieb der MultiIO Platine mit der RP6 CCPRO M128 findet ihr hier im Verzeichnis "M128" der Datei Software.zip.
RP6 CONTROL M32
Die RP6-Control-M32 (= "M32") kann über den I2C-Bus alle I2C-Sensoren und I2C-Aktoren ansteuern und hat mit ihren freien Ressourcen (Portpins) gute Voraussetzungen, um die weiteren Funktionen der Multi IO Platine zu nutzen. Dazu wird sie (wenn alle Funktionen gleichzeitig genutzt werden sollen) mit zwei Steckverbindungen angeschlossen:
- IO-Mxxx <-> I/O
- ADC-Mxxx <-> ADC
Dadurch ergeben sich umfangreiche Möglichkeiten, die Sensoren der Multi IO Platine auszulesen und Aktoren zu schalten. Die folgenden 2 Tabellen zeigen die Verbindungen:
Stecker IO-Mxxx:
| Stecker-Pin | M32-Port | M32-Funktion | Multi IO-Funktion | I/O |
| 1 | PC7 | TOSC2 | DCF77 | I |
| 2 | GND | |||
| 3 | PC5 | TDI | SHARPS_PWR | O |
| 4 | PC6 | TOSC1 | SNAKE_SWITCH | O |
| 5 | PC3 | TMS | BUZZER | O |
| 6 | PC4 | TDO | (TX *) | |
| 7 | PC2 | TCK | SNAKE_KEY / (TX *) | I |
| 8 | PD6 | ICP | (RX *) | |
| 9 | PD5 | OC1A | LFS_PWR | O |
| 10 | VDD |
Zu *) UART nicht am I/O Stecker der M32 vorhanden!
Stecker ADC-Mxxx:
| Stecker-Pin | M32-Port | M32-Funktion | Multi IO-Funktion | I/O |
| 1 | PA3 | ADC3 | TOUCH / LFS_L | I |
| 2 | PA2 | ADC2 | 3V3 alt.2 | I |
| 3 | PA4 | ADC4 | 3V3 / LFS_M | I |
| 4 | GND | |||
| 5 | PA5 | ADC5 | LFS_R | I |
| 6 | GND | |||
| 7 | PA6 | ADC6 | BUTTONS | I |
| 8 | GND | |||
| 9 | PA7 | ADC7 | 3V3 alt.1 | I |
| 10 | VDD |
Multi IO Projekt Library
Diese Avr-gcc Library für das Multi IO Projekt Board (= "MultiIO") geht von folgenden Voraussetzungen aus:
- Die RP6-Control-M32 (= "M32") wird für die Ansteuerung der MultiIO benutzt.
- Die M32 ist der I2C-Bus Master.
- Die I2C-Bus Geschwindigkeit beträgt 100 kHz.
- Alle Hardware-Komponenten der MultiIO sind aufgebaut (1).
- Alle Jumper auf der MultiIO sind in ihrer Standardstellung (2).
- Die MultiIO und die M32 sind mit dem XBUS des RP6-Systems 1:1 verbunden.
- Der Wannenstecker IO_Mxxx der MultiIO ist mit dem Wannenstecker I/O der M32 1:1 verbunden.
- Der Wannenstecker ADC_Mxxx der MultiIO ist mit dem Wannenstecker ADC der M32 1:1 verbunden.
Zu (1): Wenn nicht alle Komponenten aufgebaut sind, sind die zugehörigen
Funktionen natürlich nicht funktionsfähig und können nicht benutzt
werden.
Zu (2): Siehe hier!
Abweichende IO-Jumperstellung für die M32
Die rote Doppellinie bedeutet: Hier NIE einen Jumper aufstecken!!!
Analoge Sensoren auf dem Bumper Board
Mit dem Wahl-Jumper-Block (s.o. Abbildung "Abweichende IO-Jumperstellung für die M32"!) sind die Signalleitungen ("SigL" und "SigR") des Bumper Boards mit "SCL" und "SDA", also mit dem I2C-Bus verbunden (z.B. für SRF02). Will man stattdessen analoge Sensoren (SHARP, Radar) auf dem Bumper Board nutzen (mit der Linienfolger und Bumper Board Library für die M32), dann kann man mit dem Wahl-Jumper-Block leider keine analogen (ADC-) Eingänge dafür auswählen, weil "PF4" und "PF3" nur auf der M256 als ADC-Eingänge verfügbar sind.
Eine mögliche Lösung ist die Nutzung der beiden freien ADC-Kanäle 2 (ADC_2) und 7 (ADC_7) der M32 für die analogen Sensoren auf dem Bumper Board. Die folgende Abbildung zeigt die dafür nötigen Verbindungen:
Bumper anschliessen
Mit der M32 lassen sich leider auch die Bumper Taster auf dem Bumper Board nicht direkt auswerten (mit der Linienfolger und Bumper Board Library für die M32), weil die Signalleitungen ("ON-L" und "ON-R") der Bumper Taster an "J_ADC" Pins 4 und 8 nicht mit Portpins der M32 zu verbinden sind.
Eine mögliche Lösung ist die Nutzung der beiden freien I/O-Portpins PD6 (IO_PD6) und PC4 (IO_PC4) der M32 für die Bumper Taster auf dem Bumper Board. Die folgende Abbildung zeigt die dafür nötigen Verbindungen:
Linienfolger Sensoren (LFS) anschliessen
Will man die Linienfolger Sensoren (LFS) anstelle der 3,3V-Messung und des Touch Sensors nutzen (mit der Linienfolger und Bumper Board Library für die M32), müssen Jumper auf der MultiIO umgesteckt werden. Die folgende Abbildung zeigt den Wahl-Jumper-Block:
Hinweis: Die Jumper sind orange eingezeichnet!
Die Library (Software-Bibliothek) besteht aus drei Teilen:
- Dem Configuration Header -> Hier stehen alle Definitionen und Festlegungen, die der grundlegenden Konfiguration der MultiIO dienen. Diese Datei kann auf die eigenen Hardware-Voraussetzungen angepaßt werden, ohne dass die eigentliche Library (Header und Source) verändert werden muss.
- Dem Library Header -> Hier gibt es Definitionen, Variablen- und Funktionsdeklarationen für die Library.
- Der Library Source -> Das ist die eigentliche Library.
Configuration Header
Datei: RP6Control_MultiIO.h:
/* * **************************************************************************** * RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples * **************************************************************************** * Example: RP6Control MultiIO Library * Author(s): Dirk * **************************************************************************** * Description: * Configuration header file for new MultiIO Project Board library. * * **************************************************************************** */ #ifndef RP6CONTROL_MULTIIO_H #define RP6CONTROL_MULTIIO_H /*****************************************************************************/ // MultiIO hardwired components: // - I2C Voltage & Current Sensor (LTC2990) // - I2C Real Time Clock (RTC DS1307Z) // - I2C Temperature Sensor (TCN75A) // - I2C Servo Controller (PCA9685) // - I2C EEPROM (24LCXXX) // - 3V3 Voltage Sensor // - Touch Sensor (with NE555) // - Buttons // - LEDs // - Buzzer /*****************************************************************************/ // I2C Voltage & Current Sensor (LTC2990): #define I2C_MULTIIO_VCS_ADR 0x98 // ADR1/0 = 0/0 //#define I2C_MULTIIO_VCS_ADR 0x9a // ADR1/0 = 0/1 //#define I2C_MULTIIO_VCS_ADR 0x9c // ADR1/0 = 1/0 //#define I2C_MULTIIO_VCS_ADR 0x9e // ADR1/0 = 1/1 //#define I2C_MULTIIO_VCS_ADR 0xee // Global sync address // (Shunt resistor, current and voltage adjustment!) #define SHUNT_R 0.051 // R10 [Ohm] #define C_ADJUST 2.0 // Current adjust factor #define C_OFFSET 0.0 // Current offset [mA] #define VBAT_ADJUST 3.2 // (R13 + R14) / R14 #define VSERVO_ADJUST 2.5 // (R32 + R22) / R22 /*****************************************************************************/ // I2C Real Time Clock (RTC DS1307Z): #define I2C_MULTIIO_RTC_ADR 0xD0 // Default /*****************************************************************************/ // I2C Temperature Sensor (TCN75A): // (A1 always 0!) #define I2C_MULTIIO_TEMP_ADR 0x90 // A2/0 = 0/0 //#define I2C_MULTIIO_TEMP_ADR 0x92 // A2/0 = 0/1 //#define I2C_MULTIIO_TEMP_ADR 0x98 // A2/0 = 1/0 //#define I2C_MULTIIO_TEMP_ADR 0x9a // A2/0 = 1/1 /*****************************************************************************/ // I2C Servo Controller (PCA9685): // (A5, A4, A3, A2 always 0!) #define I2C_MULTIIO_SERVO_ADR 0x80 // A1/0 = 0/0 //#define I2C_MULTIIO_SERVO_ADR 0x82 // A1/0 = 0/1 //#define I2C_MULTIIO_SERVO_ADR 0x84 // A1/0 = 1/0 //#define I2C_MULTIIO_SERVO_ADR 0x86 // A1/0 = 1/1 //#define I2C_MULTIIO_SERVO_ADR 0xe0 // ALLCALLADR // (Servo power is connected to LED8 of the PCA9685!) #define CHSERVOPWR 9 // Servo left touch (LT), right touch (RT), middle position (MP) constants: // (Hints: - Servo impulse length [ms] = Servo position value / 204.8 // (Formula only valid for a PWM of 50 Hz!) // - Min. servo impulse (0,7 ms) = Servo position 143 // - Mid. servo impulse (1,5 ms) = Servo position 307 // - Max. servo impulse (2,3 ms) = Servo position 471 // - !!! You should NOT use servo position values < 143 or > 471 !!!) #define SERVO1_LT 205 // Servo impulse ~1ms #define SERVO1_RT 410 // Servo impulse ~2ms #define SERVO1_MP ((SERVO1_RT - SERVO1_LT) / 2 + SERVO1_LT) #define SERVO2_LT 205 #define SERVO2_RT 410 #define SERVO2_MP ((SERVO2_RT - SERVO2_LT) / 2 + SERVO2_LT) #define SERVO3_LT 205 #define SERVO3_RT 410 #define SERVO3_MP ((SERVO3_RT - SERVO3_LT) / 2 + SERVO3_LT) #define SERVO4_LT 205 #define SERVO4_RT 410 #define SERVO4_MP ((SERVO4_RT - SERVO4_LT) / 2 + SERVO4_LT) #define SERVO5_LT 205 #define SERVO5_RT 410 #define SERVO5_MP ((SERVO5_RT - SERVO5_LT) / 2 + SERVO5_LT) #define SERVO6_LT 205 #define SERVO6_RT 410 #define SERVO6_MP ((SERVO6_RT - SERVO6_LT) / 2 + SERVO6_LT) #define SERVO7_LT 205 #define SERVO7_RT 410 #define SERVO7_MP ((SERVO7_RT - SERVO7_LT) / 2 + SERVO7_LT) #define SERVO8_LT 205 #define SERVO8_RT 410 #define SERVO8_MP ((SERVO8_RT - SERVO8_LT) / 2 + SERVO8_LT) /*****************************************************************************/ // I2C EEPROM (24LCXXX): // (A2=1 not usable with 24LC1024-P!) #define I2C_MULTIIO_EEPROM_ADR 0xA0 // A2/1/0 = 0/0/0 //#define I2C_MULTIIO_EEPROM_ADR 0xA2 // A2/1/0 = 0/0/1 //#define I2C_MULTIIO_EEPROM_ADR 0xA4 // A2/1/0 = 0/1/0 //#define I2C_MULTIIO_EEPROM_ADR 0xA6 // A2/1/0 = 0/1/1 //#define I2C_MULTIIO_EEPROM_ADR 0xA8 // A2/1/0 = 1/0/0 //#define I2C_MULTIIO_EEPROM_ADR 0xAA // A2/1/0 = 1/0/1 //#define I2C_MULTIIO_EEPROM_ADR 0xAC // A2/1/0 = 1/1/0 //#define I2C_MULTIIO_EEPROM_ADR 0xAE // A2/1/0 = 1/1/1 // I2C-EEPROM storage capacity [kbit]: #define I2C_EEPROM_KBIT 32 // 24LC32-P <== Default //#define I2C_EEPROM_KBIT 64 // 24LC64-P //#define I2C_EEPROM_KBIT 128 // 24LC128-P //#define I2C_EEPROM_KBIT 256 // 24LC256-P //#define I2C_EEPROM_KBIT 512 // 24LC512-P //#define I2C_EEPROM_KBIT 1024 // 24LC1024-P // I2C-EEPROM pagesize [byte]: // ATTENTION: The pagesize must fit to the EEPROM type defined above! #define I2C_EEPROM_PAGESIZE 32 // EEPROM 32 or 64 kbit //#define I2C_EEPROM_PAGESIZE 64 // EEPROM 128 or 256 kbit //#define I2C_EEPROM_PAGESIZE 128 // EEPROM 512 kbit //#define I2C_EEPROM_PAGESIZE 256 // EEPROM 1024 kbit /*****************************************************************************/ // 3V3 Voltage Sensor: #define ADC_MULTIIO_3V3 ADC_4 // ADC-Mxxx: ADC //#define ADC_MULTIIO_3V3 ADC_7 // ADC-Mxxx: ADC //#define ADC_MULTIIO_3V3 ADC_2 // ADC-Mxxx: ADC /*****************************************************************************/ // Touch Sensor (with NE555): #define ADC_MULTIIO_TOUCH ADC_3 // ADC-Mxxx: ADC #define ADCVAL_NOTOUCH 15 #define ADCVAL_TOUCH 1020 #define ADCVAL_LIMIT_T ((ADCVAL_TOUCH - ADCVAL_NOTOUCH) / 2 + ADCVAL_NOTOUCH) /*****************************************************************************/ // Buttons: #define ADC_MULTIIO_BUTTONS ADC_6 // ADC-Mxxx: ADC #define ADCVAL_BUTTON1 14 #define ADCVAL_BUTTON2 577 #define ADCVAL_BUTTON3 736 #define ADCVAL_BUTTON4 812 #define ADCVAL_LIMIT12 ((ADCVAL_BUTTON2 - ADCVAL_BUTTON1) / 2 + ADCVAL_BUTTON1) #define ADCVAL_LIMIT23 ((ADCVAL_BUTTON3 - ADCVAL_BUTTON2) / 2 + ADCVAL_BUTTON2) #define ADCVAL_LIMIT34 ((ADCVAL_BUTTON4 - ADCVAL_BUTTON3) / 2 + ADCVAL_BUTTON3) #define ADCVAL_LIMIT40 ((1023 - ADCVAL_BUTTON4) / 2 + ADCVAL_BUTTON4) /*****************************************************************************/ // LEDs: // (Status LED1..LED4 are connected to LED15..LED12 of the PCA9685!) #define CHLED1 16 #define CHLED2 15 #define CHLED3 14 #define CHLED4 13 /*****************************************************************************/ // Buzzer: // (The IO-Mxxx plug is connected to the M32 I/O plug!) #define IO_MULTIIO_BUZZER_IN IO_PC3 // IO-Mxxx: I/O #define IO_MULTIIO_BUZZER_DDR DDRC #define IO_MULTIIO_BUZZER_PORT PORTC /*****************************************************************************/ // Other ADC channel definitions: // (Depending on jumper settings on the MultiIO Project Board!) // (The ADC-Mxxx plug is connected to the M32 ADC plug!) #define ADC_MULTIIO_LFS_L ADC_3 // ADC-Mxxx: ADC #define ADC_MULTIIO_LFS_M ADC_4 // ADC-Mxxx: ADC #define ADC_MULTIIO_LFS_R ADC_5 // ADC-Mxxx: ADC #define ADC_MULTIIO_SHARP_L ADC_2 // ADC-Mxxx: ADC #define ADC_MULTIIO_SHARP_R ADC_7 // ADC-Mxxx: ADC // Other IO portpin definitions: // (The IO-Mxxx plug is connected to the M32 I/O plug!) #define IO_MULTIIO_LFS_PWR_IN IO_PD5 // IO-Mxxx: I/O #define IO_MULTIIO_LFS_PWR_DDR DDRD #define IO_MULTIIO_LFS_PWR_PORT PORTD #define IO_MULTIIO_SHARPS_PWR_IN IO_PC5 // IO-Mxxx: I/O #define IO_MULTIIO_SHARPS_PWR_DDR DDRC #define IO_MULTIIO_SHARPS_PWR_PORT PORTC #define IO_MULTIIO_SNAKE_SWITCH_IN IO_PC6 // IO-Mxxx: I/O #define IO_MULTIIO_SNAKE_SWITCH_DDR DDRC #define IO_MULTIIO_SNAKE_SWITCH_PORT PORTC #define IO_MULTIIO_SNAKE_KEY_IN IO_PC2 // IO-Mxxx: I/O #define IO_MULTIIO_SNAKE_KEY_DDR DDRC #define IO_MULTIIO_SNAKE_KEY_PIN PINC #define IO_MULTIIO_DCF77_IN IO_PC7 // IO-Mxxx: I/O #define IO_MULTIIO_DCF77_DDR DDRC #define IO_MULTIIO_DCF77_PIN PINC #define IO_MULTIIO_BUMPER_R_IN IO_PC4 // IO-Mxxx: I/O #define IO_MULTIIO_BUMPER_R_DDR DDRC #define IO_MULTIIO_BUMPER_R_PIN PINC #define IO_MULTIIO_BUMPER_R_PORT PORTC #define IO_MULTIIO_BUMPER_L_IN IO_PD6 // IO-Mxxx: I/O #define IO_MULTIIO_BUMPER_L_DDR DDRD #define IO_MULTIIO_BUMPER_L_PIN PIND #define IO_MULTIIO_BUMPER_L_PORT PORTD /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6Control_MultiIOLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Header
Datei RP6Control_MultiIOLib.h:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
* ****************************************************************************
* Example: RP6Control MultiIO Library
* Author(s): Dirk
* ****************************************************************************
* Description:
* Header file for new MultiIO Project Board library.
*
* ****************************************************************************
*/
#ifndef RP6CONTROL_MULTIIOLIB_H
#define RP6CONTROL_MULTIIOLIB_H
/*****************************************************************************/
// MultiIO hardwired components:
// - I2C Voltage & Current Sensor (LTC2990)
// - I2C Real Time Clock (RTC DS1307Z)
// - I2C Temperature Sensor (TCN75A)
// - I2C Servo Controller (PCA9685)
// - I2C EEPROM (24LCXXX)
// - 3V3 Voltage Sensor
// - Touch Sensor (with NE555)
// - Buttons
// - LEDs
// - Buzzer
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIO.h"
/*****************************************************************************/
// I2C Voltage & Current Sensor (LTC2990):
// Registers:
#define LTC2990_STATUS 0
#define LTC2990_CONTROL 1
#define LTC2990_TRIGGER 2
#define LTC2990_TINT_MSB 4
#define LTC2990_TINT_LSB 5
#define LTC2990_V1_MSB 6
#define LTC2990_V1_LSB 7
#define LTC2990_V2_MSB 8
#define LTC2990_V2_LSB 9
#define LTC2990_V3_MSB 10
#define LTC2990_V3_LSB 11
#define LTC2990_V4_MSB 12
#define LTC2990_V4_LSB 13
#define LTC2990_VCC_MSB 14
#define LTC2990_VCC_LSB 15
// Status register bitmasks:
#define LTC2990_STATUS_DEFAULT 0
#define LTC2990_STATUS_BUSY 1
#define LTC2990_STATUS_TINT_READY 2
#define LTC2990_STATUS_V1V2_READY 4
#define LTC2990_STATUS_V2_READY 8
#define LTC2990_STATUS_V3V4_READY 16
#define LTC2990_STATUS_V4_READY 32
#define LTC2990_STATUS_VCC_READY 64
// Control register bitmasks:
#define LTC2990_CONTROL_DEFAULT 0
#define LTC2990_CONTROL_MULTIIO 0b01011010 // Mode V1-V2, V3, V4
// All measurements per mode
// Single acquisition
#define LTC2990_CONTROL_MODE02_DEFAULT 0
#define LTC2990_CONTROL_MODE34_DEFAULT 0
#define LTC2990_CONTROL_REPEAT_SINGLE 64
#define LTC2990_CONTROL_TEMP_FORMAT 128
extern double tint;
extern double cbat;
extern double vbat;
extern double vservo;
extern double vcc;
void LTC2990_write_cfg(uint8_t);
#define LTC2990_init() {LTC2990_write_cfg(LTC2990_CONTROL_MULTIIO);}
void LTC2990_run(void);
void LTC2990_read(void);
void LTC2990_calculate(void);
void LTC2990_measure(void);
/*****************************************************************************/
// I2C Real Time Clock (RTC DS1307Z):
// Registers:
#define DS1307_SECONDS 0
#define DS1307_MINUTES 1
#define DS1307_HOURS 2
#define DS1307_DAY 3
#define DS1307_DATE 4
#define DS1307_MONTH 5
#define DS1307_YEAR 6
#define DS1307_CONTROL 7
#define DS1307_RAM 8
// Control register bitmasks:
#define DS1307_CONTROL_DEFAULT 0
#define DS1307_CONTROL_RS0 1
#define DS1307_CONTROL_RS1 2
#define DS1307_CONTROL_SQWE 16
#define DS1307_CONTROL_OUT 128
enum RTCWEEKDAYS {
R_MO = 1, R_TU, R_WE, R_TH, R_FR, R_SA, R_SU
};
typedef struct {
uint16_t year; // Year
uint8_t month; // Month [1..12]
enum RTCWEEKDAYS weekday; // Weekday [1..7 = R_MO..R_SU]
uint8_t day; // Day [1..31]
} rtcdate_t;
rtcdate_t rtc_date;
typedef struct {
uint8_t dst; // Daylight-saving-time (time zone)
uint8_t hour; // Hour [0..23]
uint8_t minute; // Minute [0..59]
uint8_t second; // Second [0..59]
} rtctime_t;
rtctime_t rtc_time;
uint8_t BCD2DEC(uint8_t);
uint8_t DEC2BCD(uint8_t);
void DS1307_write_cfg(uint8_t);
void DS1307_init(void);
#define CALC_DST // Time zone will be calculated
void DS1307_read(void);
void DS1307_write(void);
uint8_t DS1307_readRAM(uint8_t);
void DS1307_writeRAM(uint8_t, uint8_t);
/*****************************************************************************/
// I2C Temperature Sensor (TCN75A):
// Registers:
#define TCN75_TEMP 0
#define TCN75_CONFIG 1
#define TCN75_HYST 2
#define TCN75_LIMIT 3
// Config register bitmasks:
#define TCN75_CONFIG_RUN 0 // Default
#define TCN75_CONFIG_SHUTDOWN 1
#define TCN75_CONFIG_COMP 0 // Default
#define TCN75_CONFIG_INT 2
#define TCN75_CONFIG_ALERT_LOW 0 // Default
#define TCN75_CONFIG_ALERT_HIGH 4
#define TCN75_CONFIG_FAULT_1 0 // Default
#define TCN75_CONFIG_FAULT_2 8
#define TCN75_CONFIG_FAULT_4 16
#define TCN75_CONFIG_FAULT_6 24
// Only for the TCN75A - high resolution and OneShot mode:
#define TCN75A_CONFIG_RES_9 0 // Default
#define TCN75A_CONFIG_RES_10 32 // 0b00100000
#define TCN75A_CONFIG_RES_11 64 // 0b01000000
#define TCN75A_CONFIG_RES_12 96 // 0b01100000
#define TCN75A_CONFIG_ONESHOT_DISABLED 0 // Default
#define TCN75A_CONFIG_ONESHOT 128 // 0b10000000
extern double temperature;
void TCN75_write_cfg(uint8_t);
#define TCN75_shutdown() {TCN75_write_cfg(TCN75_CONFIG_SHUTDOWN);}
#define TCN75_run(__CONFIG__) {TCN75_write_cfg(__CONFIG__);}
extern uint8_t temperature_low;
extern uint8_t temperature_high;
#define getTemperatureHigh() (temperature_high)
#define getTemperatureLow() (temperature_low)
void TCN75_read(void);
double TCN75_calculate(void);
double TCN75_measure(void);
/*****************************************************************************/
// I2C Servo Controller (PCA9685):
// Registers:
#define PCA9685_MODE1 0
#define PCA9685_MODE2 1
#define PCA9685_SUBADR1 2
#define PCA9685_SUBADR2 3
#define PCA9685_SUBADR3 4
#define PCA9685_ALLCALLADR 5
#define PCA9685_LED0_ON_L 6
#define PCA9685_LED0_ON_H 7
#define PCA9685_LED0_OFF_L 8
#define PCA9685_LED0_OFF_H 9
#define PCA9685_LED1_ON_L 10
#define PCA9685_LED1_ON_H 11
#define PCA9685_LED1_OFF_L 12
#define PCA9685_LED1_OFF_H 13
#define PCA9685_LED2_ON_L 14
#define PCA9685_LED2_ON_H 15
#define PCA9685_LED2_OFF_L 16
#define PCA9685_LED2_OFF_H 17
#define PCA9685_LED3_ON_L 18
#define PCA9685_LED3_ON_H 19
#define PCA9685_LED3_OFF_L 20
#define PCA9685_LED3_OFF_H 21
#define PCA9685_LED4_ON_L 22
#define PCA9685_LED4_ON_H 23
#define PCA9685_LED4_OFF_L 24
#define PCA9685_LED4_OFF_H 25
#define PCA9685_LED5_ON_L 26
#define PCA9685_LED5_ON_H 27
#define PCA9685_LED5_OFF_L 28
#define PCA9685_LED5_OFF_H 29
#define PCA9685_LED6_ON_L 30
#define PCA9685_LED6_ON_H 31
#define PCA9685_LED6_OFF_L 32
#define PCA9685_LED6_OFF_H 33
#define PCA9685_LED7_ON_L 34
#define PCA9685_LED7_ON_H 35
#define PCA9685_LED7_OFF_L 36
#define PCA9685_LED7_OFF_H 37
#define PCA9685_LED8_ON_L 38
#define PCA9685_LED8_ON_H 39
#define PCA9685_LED8_OFF_L 40
#define PCA9685_LED8_OFF_H 41
#define PCA9685_LED9_ON_L 42
#define PCA9685_LED9_ON_H 43
#define PCA9685_LED9_OFF_L 44
#define PCA9685_LED9_OFF_H 45
#define PCA9685_LED10_ON_L 46
#define PCA9685_LED10_ON_H 47
#define PCA9685_LED10_OFF_L 48
#define PCA9685_LED10_OFF_H 49
#define PCA9685_LED11_ON_L 50
#define PCA9685_LED11_ON_H 51
#define PCA9685_LED11_OFF_L 52
#define PCA9685_LED11_OFF_H 53
#define PCA9685_LED12_ON_L 54
#define PCA9685_LED12_ON_H 55
#define PCA9685_LED12_OFF_L 56
#define PCA9685_LED12_OFF_H 57
#define PCA9685_LED13_ON_L 58
#define PCA9685_LED13_ON_H 59
#define PCA9685_LED13_OFF_L 60
#define PCA9685_LED13_OFF_H 61
#define PCA9685_LED14_ON_L 62
#define PCA9685_LED14_ON_H 63
#define PCA9685_LED14_OFF_L 64
#define PCA9685_LED14_OFF_H 65
#define PCA9685_LED15_ON_L 66
#define PCA9685_LED15_ON_H 67
#define PCA9685_LED15_OFF_L 68
#define PCA9685_LED15_OFF_H 69
#define PCA9685_ALL_LED_ON_L 250
#define PCA9685_ALL_LED_ON_H 251
#define PCA9685_ALL_LED_OFF_L 252
#define PCA9685_ALL_LED_OFF_H 253
#define PCA9685_PRE_SCALE 254
#define PCA9685_TESTMODE 255
// Mode1 register bitmasks:
#define PCA9685_MODE1_ALLCALL 1
#define PCA9685_MODE1_SUB3 2
#define PCA9685_MODE1_SUB2 4
#define PCA9685_MODE1_SUB1 8
#define PCA9685_MODE1_SLEEP 16
#define PCA9685_MODE1_AI 32
#define PCA9685_MODE1_EXTCLK 64
#define PCA9685_MODE1_RESTART 128
// Mode2 register bitmasks:
#define PCA9685_MODE2_OUTNE01_DEFAULT 0
#define PCA9685_MODE2_OUTDRV 4
#define PCA9685_MODE2_OCH 8
#define PCA9685_MODE2_INVRT 16
#define F_PCA9685 25000000.0 // Int. Clock: 25 MHz
void PCA9685_init(uint16_t);
#define initServo(__FREQ__) {PCA9685_init(__FREQ__);}
void PCA9685_set(uint8_t, uint16_t);
#define setServo(__SERVO__,__POS__) {PCA9685_set(__SERVO__,__POS__);}
void PCA9685_shutdown(void);
void PCA9685_restart(void);
void setServoPower(uint8_t);
/*****************************************************************************/
// I2C EEPROM (24LCXXX):
uint8_t I2C_EEPROM_readByte(uint16_t memAddr);
void I2C_EEPROM_writeByte(uint16_t memAddr, uint8_t data);
void I2C_EEPROM_readBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length);
void I2C_EEPROM_writeBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length);
/*****************************************************************************/
// 3V3 Voltage Sensor:
extern uint16_t adc3v3;
extern double v3v3;
uint16_t get3V3Sensor(void);
double calculate3V3(void);
double measure3V3(void);
/*****************************************************************************/
// Touch Sensor (with NE555):
extern uint16_t adcTouch;
extern uint8_t touch;
uint8_t getTouch(void);
/*****************************************************************************/
// Buttons:
extern uint16_t adcButtons;
extern uint8_t releasedMultiIOButtonNumber;
extern uint8_t pressedMultiIOButtonNumber;
uint8_t getMultiIOPressedButtonNumber(void);
uint8_t checkMultiIOPressedButtonEvent(void);
uint8_t checkMultiIOReleasedButtonEvent(void);
/*****************************************************************************/
// LEDs:
void setMultiIOLEDs(uint8_t leds);
void dimMultiIOLED(uint8_t led, uint16_t duty);
void setMultiIOLED1(uint8_t led);
void setMultiIOLED2(uint8_t led);
void setMultiIOLED3(uint8_t led);
void setMultiIOLED4(uint8_t led);
/*****************************************************************************/
// Buzzer:
void Buzzer_init(void);
void buzzer(uint16_t);
/*****************************************************************************/
// MultiIO Project Board initialisation and shutdown:
void multiio_init(void);
void multiio_shutdown(void);
/*****************************************************************************/
#endif
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
*
* ---> changes are documented in the file "RP6Control_MultiIOLib.c"
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Library Source
Datei RP6Control_MultiIOLib.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
* ****************************************************************************
* Example: RP6Control MultiIO Library
* Author(s): Dirk
* ****************************************************************************
* Description:
*
* This is our new Library that contains basic routines and functions for
* accessing the hardwired components of the MultiIO Project Board.
*
* There are much more sensors, that may be connected to the MultiIO Project
* Board: Line following sensors (5x CNY70), bumpers, "Snake Vision board",
* LDRs, I2C air pressure sensor, I2C humidity sensor, DCF77 module,
* I2C IMU, I2C ultrasonic distance sensors, GPS module and more
* sensors connected to free ADCs and IOs and to the I2C bus.
* This library doesn't contain functions for these sensors, because they are
* not HARDWIRED to the MultiIO Project Board and may be connected OPTIONALLY.
*
* ****************************************************************************
*/
/*****************************************************************************/
// MultiIO hardwired components:
// - I2C Voltage & Current Sensor (LTC2990)
// - I2C Real Time Clock (RTC DS1307Z)
// - I2C Temperature Sensor (TCN75A)
// - I2C Servo Controller (PCA9685)
// - I2C EEPROM (24LCXXX)
// - 3V3 Voltage Sensor
// - Touch Sensor (with NE555)
// - Buttons
// - LEDs
// - Buzzer
/*****************************************************************************/
// Includes:
#include "RP6Control_MultiIOLib.h"
/*****************************************************************************/
// Variables:
uint8_t registerBuf[13];
/*****************************************************************************/
// I2C Voltage & Current Sensor (LTC2990):
/**
* Sends the configuration byte to a LTC2990.
*
* Input: Config byte for control register
*
* There is also a macro LTC2990_init(), which
* initializes the sensor to work in V1-V2, V3,
* V4 and Single Acquisition mode.
*
* Example:
* LTC2990_init();
*
*/
void LTC2990_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_VCS_ADR, LTC2990_CONTROL, config);
}
uint8_t vcs_tint_low;
uint8_t vcs_tint_high;
uint8_t vcs_v1_low;
uint8_t vcs_v1_high;
uint8_t vcs_v2_low;
uint8_t vcs_v2_high;
uint8_t vcs_v3_low;
uint8_t vcs_v3_high;
uint8_t vcs_v4_low;
uint8_t vcs_v4_high;
uint8_t vcs_vcc_low;
uint8_t vcs_vcc_high;
/**
* Starts a Single Acquisition measurement.
*/
void LTC2990_run(void)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_VCS_ADR, LTC2990_TRIGGER, 0);
}
/**
* Reads all data registers of the voltage & current
* sensor (VCS).
* They are stored in the variables defined above.
*/
void LTC2990_read(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_VCS_ADR, LTC2990_TINT_MSB);
I2CTWI_readBytes(I2C_MULTIIO_VCS_ADR, registerBuf, 12);
vcs_tint_high = registerBuf[0];
vcs_tint_low = registerBuf[1];
vcs_v1_high = registerBuf[2];
vcs_v1_low = registerBuf[3];
vcs_v2_high = registerBuf[4];
vcs_v2_low = registerBuf[5];
vcs_v3_high = registerBuf[6];
vcs_v3_low = registerBuf[7];
vcs_v4_high = registerBuf[8];
vcs_v4_low = registerBuf[9];
vcs_vcc_high = registerBuf[10];
vcs_vcc_low = registerBuf[11];
}
double tint; // Internal temperature
double cbat; // Current at 9V
double vbat; // Battery voltage 9V (BAT)
double vservo; // Servo voltage 5 .. 7.5V
double vcc; // Voltage 5V (VCC)
/**
* Calculates the temperature, voltage and current
* values by using the data read from the LTC2990
* with the function LTC2990_read(). The sensor is
* configured to work in V1-V2, V3, V4 and Single
* Acquisition mode.
* The result is stored in the double variables
* defined above.
*/
void LTC2990_calculate(void)
{
int16_t tmp;
double v2;
tmp = (((vcs_tint_high & 0x1f) << 8) + vcs_tint_low) << 3;
tmp = tmp / 8;
tint = tmp / 16.0; // Internal temperature [°C]
tmp = (((vcs_v2_high & 0x7f) << 8) + vcs_v2_low) << 1;
tmp = tmp / 2;
v2 = tmp * 0.01942;
v2 *= C_ADJUST; // Current adjust factor
cbat = v2 / SHUNT_R + C_OFFSET; // Battery current [mA]
tmp = ((vcs_v3_high & 0x3f) << 8) + vcs_v3_low;
vbat = tmp * 0.00030518; // Battery voltage [V]
vbat *= VBAT_ADJUST; // Voltage divider factor
tmp = ((vcs_v4_high & 0x3f) << 8) + vcs_v4_low;
vservo = tmp * 0.00030518; // Servo voltage [V]
vservo *= VSERVO_ADJUST; // Voltage divider factor
tmp = ((vcs_vcc_high & 0x3f) << 8) + vcs_vcc_low;
vcc = 2.5 + tmp * 0.00030518; // VCC [V]
}
/**
* Performs a complete measurement with the sensor
* configured to work in V1-V2, V3, V4 and Single
* Acquisition mode.
* The result is stored in the double variables
* defined above.
* This function is BLOCKING for about 200 ms!
* If you don't want a blocking measurement, you
* have to write your own function with a non
* blocking pause between starting measurement
* and reading the result or with another mode
* of operation (Repeated Acquisitions).
*/
void LTC2990_measure(void)
{
LTC2990_run(); // Start measurement
mSleep(200);
LTC2990_read(); // Read data
LTC2990_calculate(); // Calculate values
}
/*****************************************************************************/
// I2C Real Time Clock (RTC DS1307Z):
/**
* This function converts a BCD to a DEC value.
*
*/
uint8_t BCD2DEC(uint8_t bcd)
{
return ((bcd >> 4) * 10 + (bcd & 0x0f));
}
/**
* This function converts a DEC to a BCD value.
*
*/
uint8_t DEC2BCD(uint8_t dec)
{uint8_t units = dec % 10;
if (dec /= 10) {
return (units + (DEC2BCD(dec) << 4));
}
else {
return units;
}
}
/**
* Sends the configuration byte to a DS1307.
*
* Input: Config byte for control register
*
*/
void DS1307_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_RTC_ADR, DS1307_CONTROL, config);
}
/**
* Initializes the DS1307 by resetting all registers.
* Only use this function ONCE for a DS1307 without or
* with empty backup battery!
*/
void DS1307_init(void)
{
uint8_t i;
for (i = 0; i < 8; i++) {
registerBuf[i] = 0;
}
I2CTWI_transmitBytes(I2C_MULTIIO_RTC_ADR, ®isterBuf[0], 8);
}
/**
* Reads all data registers of the Real Time Clock (RTC).
* They are stored in the time & date variables defined in
* the library header.
*/
void DS1307_read(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_RTC_ADR, DS1307_SECONDS);
I2CTWI_readBytes(I2C_MULTIIO_RTC_ADR, registerBuf, 7);
rtc_time.second = BCD2DEC(registerBuf[0] & 0x7f);
rtc_time.minute = BCD2DEC(registerBuf[1]);
rtc_time.hour = BCD2DEC(registerBuf[2] & 0x3f);
rtc_date.weekday = registerBuf[3];
rtc_date.day = BCD2DEC(registerBuf[4]);
rtc_date.month = BCD2DEC(registerBuf[5]);
rtc_date.year = BCD2DEC(registerBuf[6]) + 2000;
rtc_time.dst = 0;
#ifdef CALC_DST
// Calculate MESZ (DST):
uint8_t wday = rtc_date.weekday; // Weekday [1..7 = R_MO..R_SU]
if(wday == 7) wday = 0;
if(rtc_date.month < 3 || rtc_date.month > 10) {
return;
}
if((rtc_date.day - wday >= 25)
&& (wday || rtc_time.hour >= 2)) {
if(rtc_date.month == 10)
return;
}
else {
if(rtc_date.month == 3) {
return;
}
}
rtc_time.dst = 1;
#endif
}
/**
* Writes the time & date infos in the variables defined in
* the library header to the Real Time Clock (RTC).
*/
void DS1307_write(void)
{
registerBuf[0] = DS1307_SECONDS;
registerBuf[1] = DEC2BCD(rtc_time.second);
registerBuf[2] = DEC2BCD(rtc_time.minute);
registerBuf[3] = DEC2BCD(rtc_time.hour);
registerBuf[4] = rtc_date.weekday;
registerBuf[5] = DEC2BCD(rtc_date.day);
registerBuf[6] = DEC2BCD(rtc_date.month);
registerBuf[7] = DEC2BCD(rtc_date.year - 2000);
I2CTWI_transmitBytes(I2C_MULTIIO_RTC_ADR, ®isterBuf[0], 8);
}
/**
* Reads and returns a data byte from the DS1307 RAM.
*
* Input: adr -> RAM address [0..55]
*
* Hints: - The real DS1307 RAM addresses are 8..63!
* - The RAM is nonvolatile. That means, that
* it keeps the data as long as the backup
* battery doesn't become weak!
*
*/
uint8_t DS1307_readRAM(uint8_t adr)
{
if (adr > 55) adr = 0;
I2CTWI_transmitByte(I2C_MULTIIO_RTC_ADR, (DS1307_RAM + adr));
return (I2CTWI_readByte(I2C_MULTIIO_RTC_ADR));
}
/**
* Writes a data byte to the DS1307 RAM.
*
* Input: adr -> RAM address [0..55]
* data -> Data byte [0..255]
*
* Hints: - The real DS1307 RAM addresses are 8..63!
* - The RAM is nonvolatile. That means, that
* it keeps the data as long as the backup
* battery doesn't become weak!
*
*/
void DS1307_writeRAM(uint8_t adr, uint8_t data)
{
if (adr > 55) adr = 0;
I2CTWI_transmit2Bytes(I2C_MULTIIO_RTC_ADR, (DS1307_RAM + adr), data);
}
/*****************************************************************************/
// I2C Temperature Sensor (TCN75A):
/**
* Sends the configuration byte to a TCN75A.
*
* Input: Config byte for config register
*
*/
void TCN75_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_TEMP_ADR, TCN75_CONFIG, config);
}
uint8_t temperature_low;
uint8_t temperature_high;
/**
* Reads the two data registers of the temperature
* sensor.
* They are stored in the variables temperature_low
* and _high.
*
* Hint: Depending on the sensor configuration the
* data are located different in the two bytes
* - have a look at the datasheet!
*
*/
void TCN75_read(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_TEMP_ADR, TCN75_TEMP);
I2CTWI_readBytes(I2C_MULTIIO_TEMP_ADR, registerBuf, 2);
temperature_low = registerBuf[0];
temperature_high = registerBuf[1];
}
double temperature; // Temperature [°C]
/**
* Calculates and returns the temperature value
* by using the data read from the TCN75A with
* the function TCN75_read(). The sensor is
* configured to Single Conversion and 12 bit
* measurement.
*/
double TCN75_calculate(void)
{
uint8_t templow;
double temp;
templow = getTemperatureLow();
if (templow & 128) // Calculate temperature
templow = (templow & 63) - 127;
else
templow = templow & 63;
temp = templow + (0.0625 * (getTemperatureHigh() >> 4));
return temp;
}
/**
* Performs a 12 bit measurement and returns the
* temperature [°C].
* This function is BLOCKING for about 250 ms!
* If you don't want a blocking measurement, you
* have to write your own function with a non
* blocking pause between starting measurement
* and reading the result or with another mode
* of operation (Continuous Conversion).
*/
double TCN75_measure(void)
{
TCN75_run(TCN75A_CONFIG_RES_12); // Start measurement
mSleep(250);
TCN75_shutdown(); // Stop measurement
TCN75_read(); // Read data
return (TCN75_calculate()); // Calculate value
}
/*****************************************************************************/
// I2C Servo Controller (PCA9685):
/**
* Call this once before using the servo function.
*
* Input: PWM frequency [40..1000 Hz]
*
* Hints: - Default servo frequency is 50 Hz!
* - The servo power is NOT switched on by
* this function!
*
* There is also a macro initServo(freq), which
* does exactly the same as this function.
*
* Example:
* initServo(50);
*
*/
void PCA9685_init(uint16_t freq)
{
if ((freq < 40) || (freq > 1000)) freq = 50;
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE2);
uint8_t last_mode = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
last_mode &= ~PCA9685_MODE2_INVRT; // Clear INVRT bit
last_mode |= PCA9685_MODE2_OUTDRV; // Set OUTDRV bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE2, last_mode);
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1);
last_mode = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
last_mode |= PCA9685_MODE1_AI; // Set AI bit
uint8_t mode1 = last_mode;
mode1 |= PCA9685_MODE1_SLEEP; // Set SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
uint8_t prescale = (uint8_t) (F_PCA9685 / 4096 / freq - 0.5);
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_PRE_SCALE, prescale);
last_mode &= ~PCA9685_MODE1_SLEEP; // Clear SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, last_mode);
mSleep(1);
last_mode |= PCA9685_MODE1_RESTART; // Clear RESTART bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, last_mode);
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_LED8_ON_H, 0x10);
return;
}
/**
* This is the servo position set function.
*
* Input: servo -> Servo number [1..8, 10..16]
* pos -> Servo position [SERVOx_LT..SERVOx_RT]
*
* Hints: - Servo number 9 cannot be set by this function!
* Please use setServoPower() function instead!
* - A servo position of 205 means 1 ms servo impulse,
* a position of 410 means a 2 ms servo impulse!
* You may calculate the servo impulse length by:
* ==> Impulse [ms] = servo position / 204.8 <==
* (Formula only valid for a PWM of 50 Hz!)
*
* There is also a macro setServo(servo, pos), which
* does exactly the same as this function.
*
* Example:
* setServo(2,300);
*
*/
void PCA9685_set(uint8_t servo, uint16_t pos)
{
if ((servo == 0) || (servo == CHSERVOPWR) || (servo > 16))
return;
uint8_t reg = servo * 4 + 4; // Register LEDx_OFF_L
I2CTWI_transmit3Bytes(I2C_MULTIIO_SERVO_ADR, reg, (pos & 0x00ff), (pos >> 8));
}
/**
* If the servos are not moving for a while, the
* servo function can be stopped with this
* function (PCA9685 set to sleep mode).
*
* Hint: The servo power is NOT switched off by
* this function!
*
*/
void PCA9685_shutdown(void)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0x10);
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1);
uint8_t mode1 = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
mode1 |= PCA9685_MODE1_SLEEP; // Set SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
}
/**
* If the servo function was stopped with the
* function PCA9685_shutdown() before, it can be
* (re)started again with this function.
*
* Hint: The servo power is NOT switched on by
* this function!
*
*/
void PCA9685_restart(void)
{
I2CTWI_transmitByte(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1);
uint8_t mode1 = I2CTWI_readByte(I2C_MULTIIO_SERVO_ADR);
if (mode1 & PCA9685_MODE1_RESTART) { // RESTART bit set?
mode1 &= ~PCA9685_MODE1_SLEEP; // Clear SLEEP bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
mSleep(1);
mode1 |= PCA9685_MODE1_RESTART; // Clear RESTART bit
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_MODE1, mode1);
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0);
}
}
/**
* With this function you can switch the servo
* power on or off, if the servo power jumper on
* the board enables this feature.
*
* Input: pwr -> 0 (false) = servo power off
* >0 (true) = servo power on
*
* Hints: - If connected servos are not used, you
* should always switch the servo power off
* to save energy!
* - The PCA9685 is NOT restarted or put into
* shutdown mode by this function!
*
*/
void setServoPower(uint8_t pwr)
{
if (pwr > 0)
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_LED8_OFF_H, 0);
else
I2CTWI_transmit2Bytes(I2C_MULTIIO_SERVO_ADR, PCA9685_LED8_OFF_H, 0x10);
}
/*****************************************************************************/
// I2C EEPROM (24LCXXX):
// The following EEPROM types may be used:
// EEPROM Size: Chip Pagesize
// - 32 kbit: ST24LC32A 32
// - 64 kbit: ST24LC64 32
// - 128 kbit: ST24LC128 64
// - 256 kbit: ST24LC256 64
// - 512 kbit: ST24LC512 128
// - 1024 kbit: AT24LC1024 256
// ! You may choose the EEPROM type in the I2C EEPROM !
// ! section of the RP6Control_MultiIO.h file! !
/**
* Reads a single Byte from the EEPROM.
*/
uint8_t I2C_EEPROM_readByte(uint16_t memAddr)
{
uint8_t data;
I2CTWI_transmit2Bytes(I2C_MULTIIO_EEPROM_ADR, (memAddr >> 8), memAddr);
data = I2CTWI_readByte(I2C_MULTIIO_EEPROM_ADR);
return data;
}
/**
* Write a single data byte to the specified EEPROM address.
*/
void I2C_EEPROM_writeByte(uint16_t memAddr, uint8_t data)
{
I2CTWI_transmit3Bytes(I2C_MULTIIO_EEPROM_ADR, (memAddr >> 8), memAddr, data);
mSleep(5);
}
/**
* Reads "length" Bytes into the Buffer "buffer" from startAddr on.
* You can read the complete EEPROM into a buffer at once - if it is large enough.
* (But you only have 2KB SRAM on a MEGA32 ;) )
* If "length" is higher than I2CTWI_BUFFER_REC_SIZE defined in RP6I2CmasterTWI.h,
* you have to adapt I2CTWI_BUFFER_REC_SIZE to the highest used "length" value!
*/
void I2C_EEPROM_readBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length)
{
I2CTWI_transmit2Bytes(I2C_MULTIIO_EEPROM_ADR, (startAddr >> 8), startAddr);
I2CTWI_readBytes(I2C_MULTIIO_EEPROM_ADR, &buffer[0], length);
}
/**
* Write "length" Bytes from the Buffer to the EEPROM.
* YOU CAN ONLY WRITE MAXIMAL [I2C_EEPROM_PAGESIZE] BYTES AT ONCE!!!
* This is the Pagesize!
* You can NOT cross a page boundary!
* If (length + 2) is higher than I2CTWI_BUFFER_SIZE defined in RP6I2CmasterTWI.h,
* you have to adapt I2CTWI_BUFFER_SIZE to the highest used (length + 2) value!
*/
void I2C_EEPROM_writeBytes(uint16_t startAddr, uint8_t *buffer, uint8_t length)
{
uint8_t i, addrbuffer[length + 2];
addrbuffer[0] = (startAddr >> 8);
addrbuffer[1] = startAddr;
for(i = 0; i < length; i++) {
addrbuffer[i + 2] = buffer[i];
}
I2CTWI_transmitBytes(I2C_MULTIIO_EEPROM_ADR, &addrbuffer[0], (length + 2));
mSleep(5);
}
/*****************************************************************************/
// 3V3 Voltage Sensor:
uint16_t adc3v3; // 3V3 voltage sensor ADC value
/**
* This function reads and returns the ADC value of
* the 3V3 voltage sensor. The value is also stored
* in adc3v3.
*
*/
uint16_t get3V3Sensor(void)
{
adc3v3 = readADC(ADC_MULTIIO_3V3);
return adc3v3;
}
double v3v3; // 3V3 voltage [V]
/**
* Calculates and returns the 3.3V voltage value
* by using the data read from the 3V3 voltage
* sensor with the function get3V3Sensor().
*
*/
double calculate3V3(void)
{
return (5.0 / 1024.0 * adc3v3);
}
/**
* Measures and returns the 3.3V voltage value.
* The ADC value of the 3V3 voltage sensor is also
* stored in adc3v3.
*
*/
double measure3V3(void)
{
adc3v3 = readADC(ADC_MULTIIO_3V3);
return (5.0 / 1024.0 * adc3v3);
}
/*****************************************************************************/
// Touch Sensor (with NE555):
uint16_t adcTouch; // Touch sensor ADC value
uint8_t touch = 0; // True (1), if touched
/**
* Checks if the touch sensor antenna is touched - returns 1,
* if touched or 0, if the antenna is NOT touched.
*
*/
uint8_t getTouch(void)
{
adcTouch = readADC(ADC_MULTIIO_TOUCH);
if (adcTouch > ADCVAL_LIMIT_T) return 1;
else return 0;
}
/*****************************************************************************/
// Buttons:
uint16_t adcButtons; // Keypad ADC value
uint8_t releasedMultiIOButtonNumber; // Released (last pressed) button
uint8_t pressedMultiIOButtonNumber; // Actually pressed button
/**
* Checks which button is pressed - returns the button number,
* or 0, if no button is pressed.
* Maybe the values of ADCVAL_LIMITxx have to change because
* of variations in the resistors of the keypad! This is done
* in RP6Control_MultiIO.h, if you define other ADC values for
* your 4 buttons in ADCVAL_BUTTON1..ADCVAL_BUTTON4.
*
*/
uint8_t getMultiIOPressedButtonNumber(void)
{
adcButtons = readADC(ADC_MULTIIO_BUTTONS);
if(adcButtons < 1020) {
nop();
nop();
nop();
adcButtons += readADC(ADC_MULTIIO_BUTTONS);
adcButtons >>= 1;
}
if(adcButtons < ADCVAL_LIMIT12)
return 1;
if(adcButtons < ADCVAL_LIMIT23)
return 2;
if(adcButtons < ADCVAL_LIMIT34)
return 3;
if(adcButtons < ADCVAL_LIMIT40)
return 4;
return 0;
}
/**
* This function has to be called frequently out of the
* main loop and checks if a button is pressed! It only returns
* the button number a single time, DIRECTLY when the button is
* pressed.
*
* This is useful for non-blocking keyboard check in the
* main loop. You don't need something like
* "while(getMultiIOPressedButtonNumber());" to wait for the
* button to be released again!
*/
uint8_t checkMultiIOPressedButtonEvent(void)
{
static uint8_t pressed_button = 0;
if(pressed_button) {
if(!getMultiIOPressedButtonNumber())
pressed_button = 0;
}
else {
pressed_button = getMultiIOPressedButtonNumber();
if(pressed_button)
return pressed_button;
}
return 0;
}
/**
* This function has to be called frequently out of
* the main loop and checks if a button is pressed AND
* released. It only returns the button number a single
* time, AFTER the button has been released.
*
* This is useful for non-blocking keyboard check in the
* main loop. You don't need something like
* "while(getMultiIOPressedButtonNumber());" to wait for the
* button to be released again!
*/
uint8_t checkMultiIOReleasedButtonEvent(void)
{
static uint8_t released_button = 0;
if(released_button) {
if(!getMultiIOPressedButtonNumber()) {
uint8_t tmp = released_button;
released_button = 0;
return tmp;
}
}
else
released_button = getMultiIOPressedButtonNumber();
return 0;
}
/*****************************************************************************/
// LEDs:
/**
* Set the 4 status LEDs of the MultiIO.
*
* Example:
* setMultiIOLEDs(0b1010);
* // this clears LEDs 1 and 3
* // and sets LEDs 2 and 4!
*
*/
void setMultiIOLEDs(uint8_t leds)
{
if (leds & 0b00000001) PCA9685_set(CHLED1, 4095);
else PCA9685_set(CHLED1, 0);
if (leds & 0b00000010) PCA9685_set(CHLED2, 4095);
else PCA9685_set(CHLED2, 0);
if (leds & 0b00000100) PCA9685_set(CHLED3, 4095);
else PCA9685_set(CHLED3, 0);
if (leds & 0b00001000) PCA9685_set(CHLED4, 4095);
else PCA9685_set(CHLED4, 0);
}
/**
* Dim the 4 status LEDs of the MultiIO.
*
* Input: led -> LED number [1..4]
* duty -> Duty cycle [0..4095]
*
* Example:
* dimMultiIOLED(2,2048);
* // this dims LED2 with a
* // duty cycle of 50% !
*
*/
void dimMultiIOLED(uint8_t led, uint16_t duty)
{
if (led == 1) PCA9685_set(CHLED1, duty);
if (led == 2) PCA9685_set(CHLED2, duty);
if (led == 3) PCA9685_set(CHLED3, duty);
if (led == 4) PCA9685_set(CHLED4, duty);
}
/**
* Set ONLY LED1, don't change anything for the other LEDs.
*
* Input: led -> 0 (false) = LED off
* >0 (true) = LED on
*
*/
void setMultiIOLED1(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED1, 4095);
else PCA9685_set(CHLED1, 0);
}
/**
* Set ONLY LED2, don't change anything for the other LEDs.
*/
void setMultiIOLED2(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED2, 4095);
else PCA9685_set(CHLED2, 0);
}
/**
* Set ONLY LED3, don't change anything for the other LEDs.
*/
void setMultiIOLED3(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED3, 4095);
else PCA9685_set(CHLED3, 0);
}
/**
* Set ONLY LED4, don't change anything for the other LEDs.
*/
void setMultiIOLED4(uint8_t led)
{
if (led > 0) PCA9685_set(CHLED4, 4095);
else PCA9685_set(CHLED4, 0);
}
/*****************************************************************************/
// Buzzer:
/**
* Call this once before using the buzzer.
*
*/
void Buzzer_init(void)
{
IO_MULTIIO_BUZZER_DDR |= IO_MULTIIO_BUZZER_IN;
IO_MULTIIO_BUZZER_PORT &= ~IO_MULTIIO_BUZZER_IN;
}
/**
* You can use this function to make the buzzer beep ;)
* with a frequency of about 1000 Hz.
* This function is BLOCKING and generates a delay for the
* sound.
*
* Input: time -> sound length [ms]
*
* Example:
* buzzer(150);
*
*/
void buzzer(uint16_t time)
{
if (!time) return;
while (time--) {
IO_MULTIIO_BUZZER_PORT |= IO_MULTIIO_BUZZER_IN;
sleep(5);
IO_MULTIIO_BUZZER_PORT &= ~IO_MULTIIO_BUZZER_IN;
sleep(5);
}
}
/*****************************************************************************/
/*****************************************************************************/
// MultiIO Project Board initialisation and shutdown:
/**
* You MUST call this function at the beginning of a
* main program, that uses the MultiIO Project Board.
*
*/
void multiio_init(void)
{
// Voltage & current sensor:
LTC2990_init(); // Init VCS
// Servo Controller:
PCA9685_init(50); // Init PWM 50 Hz
// Servo power:
setServoPower(0); // Servo power off
// Buzzer:
Buzzer_init(); // Init buzzer
}
/**
* If you don't use any function of the MultiIO
* Project Board, you can put the MultiIO into
* "SHUTDOWN MODE". In this mode the electric
* power consumption is very low to save energy.
*
*/
void multiio_shutdown(void)
{
// LEDs:
setMultiIOLEDs(0b0000); // MultiIO LEDs off
// Servo power:
setServoPower(0); // Servo power off
// Servo Controller:
PCA9685_shutdown(); // Shutdown PCA9685
// Temperature Sensor:
TCN75_shutdown(); // Shutdown TCN75A
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.2 (shutdown mode added) 28.03.2013 by Dirk
* - v. 1.1 (LTC2990 current bug fixed) 26.03.2013 by Dirk
* - v. 1.0 (initial release) 18.03.2013 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6Control_MultiIO_01 ... SRC += RP6Control_MultiIOLib.c ...
Datei RP6Control_MultiIO_01.c:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
* ****************************************************************************
* Example: RP6Control MultiIO
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a first test for the MultiIO Project Board.
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h" // The RP6 Control Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // Include the I2C-Bus Master Library
/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6Control MultiIO library":
// (This is the library for accessing the MultiIO Project Board!)
#include "RP6Control_MultiIOLib.h"
/*****************************************************************************/
/**
* Write a floating point number to the UART.
*
* Example:
*
* // Write a floating point number to the UART (no exponent):
* writeDouble(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDouble(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString(&buffer[0]);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
* The most common mistakes are:
* - using the wrong address for the slave
* - slave not active or not connected to the I2C-Bus
* - too fast requests for a slower slave
* Be sure to check this if you get I2C errors!
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR --> TWI STATE IS: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6Control(); // Always call this first! The Processor will not
// work correctly otherwise.
initLCD(); // Initialize the LC-Display (LCD)
// Always call this before using the LCD!
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
writeString_P("\n\nRP6Control Multi IO Selftest 1!\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handler:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
setLEDs(0b1111);
// Write a text message to the LCD:
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD(" RP6Control M32", "Example Program");
mSleep(2500);
showScreenLCD(" RP6 Multi IO", " Selftest 1");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
uint8_t onoff = 0;
uint16_t servopos = SERVO1_LT;
startStopwatch1();
// IMPORTANT:
multiio_init(); // MultiIO init!!!
//setServoPower(1); // Servo power ON!
// ----------------------------------------------
// Set RTC once (battery empty or not existing:
rtc_time.second = 0;
rtc_time.minute = 0;
rtc_time.hour = 12; // 12:00
rtc_date.weekday = R_TH;
rtc_date.day = 28;
rtc_date.month = 3;
rtc_date.year = 2013; // Do, 28.3.2013
DS1307_write();
// Remove this, if RTC is set and running!!!
// ----------------------------------------------
// EEPROM test:
writeString_P("\nWriting 128 to EEPROM address 5:\n");
I2C_EEPROM_writeByte(5, 128);
mSleep(500);
writeString_P("Done!\n");
writeString_P("\nReading EEPROM address 5:\n");
uint8_t tmp = I2C_EEPROM_readByte(5);
mSleep(500);
I2C_EEPROM_writeByte(5, 0);
writeString_P("Done!\n");
writeString_P("EEPROM address 5 content: ");
writeInteger(tmp, DEC);
writeString_P("\n");
// Buzzer test:
buzzer(330);
mSleep(200);
buzzer(330);
mSleep(200);
buzzer(330);
mSleep(1000);
buzzer(330);
mSleep(200);
buzzer(330);
mSleep(200);
buzzer(330);
while(true)
{
if(getStopwatch1() > 1000) // 1s
{
if (onoff) onoff = 0;
else onoff = 1;
// Buttons ADC test:
clearLCD();
pressedMultiIOButtonNumber = getMultiIOPressedButtonNumber();
setCursorPosLCD(0, 0);
writeStringLCD("Button: ");
writeIntegerLCD(pressedMultiIOButtonNumber, DEC);
setCursorPosLCD(1, 0);
writeStringLCD("ADC: ");
writeIntegerLCD(adcButtons, DEC);
// 3V3 voltage sensor ADC test:
v3v3 = measure3V3();
writeString("\n3V3 Voltage: ");
writeDouble(v3v3, 4, 1);
writeString("V\nADC 3V3: ");
writeInteger(adc3v3, DEC);
// Touch sensor ADC test:
touch = getTouch();
if (touch) writeString("\nTOUCHED!!!");
else writeString("\nNOT touched.");
writeString("\nADC Touch: ");
writeInteger(adcTouch, DEC);
// Temperature sensor test:
temperature = TCN75_measure(); // Measure
writeString("\nTemperature: ");
writeDouble(temperature, 5, 1);
writeString("°\n");
// Servo controller test:
// LEDs:
if (onoff) {
setMultiIOLED1(1);
setMultiIOLED2(0);
setMultiIOLED3(0);
setMultiIOLED4(1);
}
else
setMultiIOLEDs(0b0110);
// Servo 1:
setServo(1, servopos);
servopos += 10;
if (servopos > SERVO1_RT) servopos = SERVO1_LT;
// RTC test:
DS1307_read();
writeString("RTC: ");
writeIntegerLength(rtc_time.hour, DEC, 2);
writeString(":");
writeIntegerLength(rtc_time.minute, DEC, 2);
writeString(":");
writeIntegerLength(rtc_time.second, DEC, 2);
writeString(" ");
writeIntegerLength(rtc_date.day, DEC, 2);
writeString(".");
writeIntegerLength(rtc_date.month, DEC, 2);
writeString(".");
writeIntegerLength(rtc_date.year, DEC, 4);
writeString("\n");
// Voltage & current sensor test:
LTC2990_measure();
writeString("Temperature: ");
writeDouble(tint, 5, 1);
writeString("°\n");
writeString("BAT Current: ");
writeDouble(cbat, 6, 1);
writeString("mA\nBAT Voltage: ");
writeDouble(vbat, 4, 1);
writeString( "V\nSERVO Volt.: ");
writeDouble(vservo, 4, 1);
writeString( "V\nVCC Voltage: ");
writeDouble(vcc, 4, 1);
writeString("V\n");
// MultiIO shutdown:
if (pressedMultiIOButtonNumber == 4) {
writeString("\nPress button 1 for MultiIO SHUTDOWN");
writeString("\nor any other button to continue!!!\n");
do {
mSleep(1);
releasedMultiIOButtonNumber = checkMultiIOReleasedButtonEvent();
task_I2CTWI();
} while (!releasedMultiIOButtonNumber);
if (releasedMultiIOButtonNumber == 1) {
writeString("\nPlease wait for MultiIO SHUTDOWN...\n");
multiio_shutdown();
mSleep(3000);
writeString("\nThe MultiIO now is in SHUTDOWN MODE!!!\n");
mSleep(1000);
writeString("\nRESET the M32 microcontroller now...\n\n");
while(true) {};
}
}
setStopwatch1(0);
}
task_I2CTWI();
}
return 0;
}
Erklärung
DCF77-Library
Die "Funkuhr-Zeit" kann empfangen und ausgewertet werden, wenn ein DCF77-Empfänger (z.B. CONRAD 641138) mit dem dafür vorgesehenen Anschluß "DCF77" auf der Multi IO Platine verbunden ist.
Die (RP6 BASE und) RP6 CONTROL M32 DCF77 Library findet ihr HIER!
Geändert werden muss der Eingangs-Portpin. Dazu schaut euch die Datei RP6ControlDCFLib.c an.
Die folgenden Zeilen am Anfang der Datei im Abschnitt "Defines":
// RP6 Control DCF Receiver Connection: #define DCF_PORT PORTD #define DCF_DDR DDRD #define DCF_PIN PIND #define DCF_IN EINT1 // PIND2 XBUS Pin 8
... müssen so geändert werden:
// RP6 Control DCF Receiver Connection: #define DCF_PORT PORTC #define DCF_DDR DDRC #define DCF_PIN PINC #define DCF_IN IO_PC7 // PINC7 I/O Pin 1
Servo-Library
Auf der MultiIO ist ein 10-poliger Wannenstecker "ServosM32" und ein 8x3-poliger Servo-Anschluß-Stecker vorgesehen, um 8 Servos direkt an die RP6Control M32 anschließen zu können. Dazu muss der I/O-Wannenstecker der M32 mit dem o.g. Wannenstecker für die Servos verbunden werden. Man kann dann bis zu 8 Servos an den Servo-Anschluß-Stecker der MultiIO anstecken.
Dieser M32 Servo-Anschluß auf der MultiIO dient also NUR dazu, das Anschließen von Servos zu erleichtern. Die MultiIO steuert diese Servos NICHT! Sie stellt allerdings die Stromversorgung für die Servos zur Verfügung. Das bedeutet, dass die Servo-Spannung auf der MultiIO eingeschaltet werden muss, wenn man diese Steckverbindung nutzen will.
Wenn man die RP6Control M32 Library für 8 Servos nicht verändern will, muss man den Jumper "J_Servos-On" auf der MultiIO aufstecken, siehe folgende Abbildung!
Die RP6Control M32 Library für 8 Servos findet ihr HIER!
Wo die 8 Servos (SERVO1..SERVO8) anzuschließen sind zeigt diese Abbildung:
Weitere Software
Weitere Software einschließlich Libraries für den Betrieb der MultiIO Platine mit der RP6 CONTROL M32 findet ihr hier im Verzeichnis "M32" der Datei Software.zip.
RP6 BASE
Die RP6(v2) BASE (= "RP6") kann über den I2C-Bus alle I2C-Sensoren und I2C-Aktoren ansteuern. Da der RP6 nur über wenige freie Portpins verfügt, muss man eine Auswahl treffen, welche weiteren Funktionen man nutzen will.
Es gibt auf dem RP6 auch keine passenden Wannenstecker (wie auf der M32, M128 und M256), um auf einfache Weise eine mehrpolige Verbindung zu den IO-Mxxx, ADC-Mxxx oder ADC-M256 Steckern der Multi IO Platine herzustellen. Stattdessen kann man einzelne Portpins des RP6 mit der Multi IO Platine verbinden. Diese Verbindung kann auch über den USRBUS erfolgen. Die folgende Tabelle zeigt mögliche Verbindungen:
| RP6-Port | RP6-Funktion | RP6-Stecker:Pin | I/O | Bemerkungen | Z.B. für Multi IO-Funktion |
| PA0 | ADC0 | ADC0:3 | I/O | Freier ADC oder IO | BUTTONS, TOUCH, SHARPS_L/R, 3V3 ... |
| PA1 | ADC1 | ADC1:3 | I/O | Freier ADC oder IO | LFS_L/M/R, SNAKE_L/R, LDR1/2 ... |
| PA4 | ADC4 | XBUS:8 | I/O | XBUS INT1! Falls nicht für den I2C-Bus benutzt: freier IO * | SNAKE_SWITCH, SNAKE_KEY ... |
| PB1 | T1 | IO4 *** | I | Status LED5! Zusätzlich als Input z.B. für Taster nutzbar * | BUMPER_L/R ... |
| PB7 | SCK | IO3 **, ISP:7 | I | Status LED4! Zusätzlich als Input z.B. für Taster nutzbar * | BUMPER_L/R ... |
| PC4 | TDO | IO1 | I | Status LED1! Zusätzlich als Input z.B. für Taster nutzbar * | SNAKE_KEY ... |
| PC5 | TDI | IO2 | I | Status LED2! Zusätzlich als Input z.B. für Taster nutzbar * | DCF77 ... |
| PD0 | RXD | PRG/U:2 | I/O | UART RX! Nutzbar NUR, wenn USB-Interface abgezogen, als UART RX oder freier IO | GPS-Tx, SHARPS_PWR ... |
| PD1 | TXD | PRG/U:3 | I/O | UART TX! Nutzbar NUR, wenn USB-Interface abgezogen, als UART TX oder freier IO | GPS-Rx, LFS_PWR ... |
Zu *) ACHTUNG: Bestimmte Beschaltung erforderlich! Sollte nur genutzt werden, wenn man sich gut mit dem RP6 auskennt!
Zu **) RP6v2: IO4!
Zu ***) RP6v2: IO5!
Multi IO Projekt Library
Aus den genannten Gründen kann es keine generelle Multi IO Projekt Library für die RP6 BASE geben! Ich bin gern dabei, wenn jemand die M32 Library für die RP6 BASE umschreiben möchte! Auf jeden Fall lassen sich alle Teile der M32 Library für die RP6 BASE übernehmen, die die I2C-Sensoren und -Aktoren der MultiIO auslesen/ansteuern. Die können problemlos mit der RP6 BASE genutzt werden, wenn die BASE als I2C-Master konfiguriert wird. Folgende Sensoren/Aktoren lassen sich über I2C mit der RP6 BASE auslesen/ansteuern: - Spannungs- und Stromsensor (LTC2990) - Echtzeituhr (RTC DS1307Z) - Temperatursensor (TCN75A) - Servo Controller (PCA9685) - EEPROM (24LCXXX) - Ultraschalldistanzsensoren (SRF02) - Luftdrucksensor (BMP085) - Feuchte-, Temperatursensor (HYT-221) - 2D-Kompass (HDMM01) - 9D-IMU (MinIMU-9 v2) - AM-/FM-Radio mit RDS (Si4735) Folgender Sensor läßt sich über den UART der RP6 BASE aus- lesen: - GPS-Empfänger (NL-552ETTL)
Technische Informationen
I2C-Adressen
I2C-Adressen der MultiIO:
| I2C-Baustein | I2C- Adresse * |
Alternative I2C-Adressen | Verwendung |
| I2C Voltage & Current Sensor (LTC2990) | 0x98 ² | 0x9a ², 0x9c, 0x9e, 0xee | Voltage & Current Sensor |
| I2C Real Time Clock (RTC DS1307Z) | 0xd0 ² | Real Time Clock | |
| I2C Temperature Sensor (TCN75A) | 0x90 ² | 0x92 ², 0x98 ², 0x9a ² | Temperature Sensor |
| I2C Servo Controller (PCA9685) | 0x80 | 0x82, 0x84, 0x86, 0xe0 ² | Servo Controller |
| I2C EEPROM (24LCXXX) | 0xa0 ² | 0xa2 ², 0xa4 ², 0xa6 ², 0xa8, 0xaa, 0xac, 0xae | EEPROM |
Zu *) Standard-Adresse.
Zu ²) Adress-Konflikt mit dem RP6-System möglich (siehe hier)!
Verschaltung der Jumperblöcke
Auf der MultiIO Zusatzplatine für den RP6(v2) gibt es vier Jumperblöcke (J_IO, J_ADC, J_ADIO, J_WAHL), auf die je nach angeschlossenem Prozessorsystem (M32, M128, M256) und je nach Konfiguration Steckbrücken aufgesteckt werden müssen. Die Lage dieser Steckbrücken (Jumper) wurde in den vorangegangenen Kapiteln beschrieben. Wenn man selbst prüfen möchte, ob die eigene Konfiguration stimmt, fehlt häufig die Information, wie die Jumperblöcke untereinander und mit den Anschlußsteckern (IO-Mxxx, ADC-Mxxx, ADC-M256) verschaltet sind. Außer diesen Jumperblöcken sind noch die Stiftleisten für den Anschluß von GPS (5-polige Stiftleiste mit 5V/2xGND/Tx/Rx und 3-polige Stiftleiste mit GND/Tx/Rx5V), DCF77, Bumper, CNY70 (LFS), Snake, LDRs, Gyro & Kompass abgebildet, auf die natürlich keine Steckbrücken gesteckt werden dürfen. Die Verschaltung dieser Komponenten zeigt die folgende Abbildung:
Beispiel 3,3V-Messung
Wie kann man aus diesem "Wirrwarr" von (türkisfarbenen) Verbindungen auf der MultiIO Hinweise auf eine eigene Konfiguration bzw. auf die Position von Jumpern bekommen?
Am Beispiel der 3,3V-Messung soll das einmal verdeutlicht werden:
Auf der MultiIO wird ja u.a. auch vom IC MIC5219-3.3 eine 3,3V-Spannung erzeugt, die für 3,3V-Sensoren (z.B. I2C-Luftdrucksensor BMP085) genutzt werden kann. Diese Spannung kann man (zu Kontrollzwecken) auch messen. Die folgende Abbildung zeigt die (hellgrünen) Verbindungen zwischen den Jumperblöcken, die hierfür eine Rolle spielen:
Verfolgt man die hellgrünen 3,3V-Mess-Verbindungen im Bild von oben nach unten, dann wird die auf der MultiIO erzeugte 3,3V-Spannung (über einen Widerstand R36) zum Pin "3V3" des Wahl-Jumper-Blocks (J_WAHL) geführt. Von dort gelangt sie direkt zu zwei Pins des Jumperblocks J_ADC (beschriftet mit "3V3?"). Setzt man auf J_ADC ganz links ("Pins 2") einen Jumper, dann endet die 3,3V-Mess-Verbindung an Pin 2 des ADC-Mxxx Steckers. Setzt man stattdessen den Jumper auf J_ADC eine Position weiter nach rechts ("Pins 9"), dann endet die Mess-Verbindung an Pin 9 des ADC-Mxxx Steckers. Für diese beiden Verbindungen ist kein Jumper auf J_WAHL erforderlich.
Durch einen Jumper auf die Pins "3V3" und "AD3" von J_WAHL (Standard-Stellung der Jumper!) wird die 3,3V-Mess-Verbindung zu Pin 3 von J_ADC und von dort weiter zu Pin 3 des ADC-Mxxx Steckers geführt.
Steckt man den Jumper von J_WAHL auf die Pins "PF4" und "3V3" um, dann gelangt die 3,3V-Mess-Leitung zu Pin "SnT PF4" des J_ADIO Jumperblocks und von dort (falls ein Jumper auf die Pins in Position 4 von links von J_ADIO gesteckt wird) zu Pin 9 des ADC-M256 Steckers.
Das bedeutet, dass die 3,3V-Mess-Spannung der MultiIO durch Umstecken von Jumpern zu insgesamt vier Positionen geführt werden kann:
- ADC-Mxxx Stecker Pin 2
- ADC-Mxxx Stecker Pin 9
- ADC-Mxxx Stecker Pin 3
- ADC-M256 Stecker Pin 9
Wenn man nun herausfinden will, mit welchem Portpin bzw. ADC-Kanal die 3,3V-Spannung gemessen werden kann, dann geben die Tabellen:
- M256 -> Stecker ADC-Mxxx und Stecker ADC-M256
- M128 -> Stecker ADC-Mxxx
- M32 -> Stecker ADC-Mxxx
... darüber Auskunft. Nehmen wir an, die M256 ist an die MultiIO angeschlossen:
Dann entsprechen die Pins 3 und 9 des ADC-Mxxx Steckers den ADC-Kanälen 9 und 12 der M256. Der Pin 2 des ADC-Mxxx Steckers entspricht bei der M256 GND und ist nicht zur 3,3V-Messung nutzbar. Am ADC-M256 Stecker entspricht der Pin 9 dem ADC-Kanal 4 der M256.
Im Ergebnis ist also die 3,3V-Spannung auf der MultiIO abhängig von der Jumperstellung mit den ADC-Kanälen 4, 9 und 12 der M256 messbar.
Wenn man sich für eine bestimmte Jumperstellung entschieden hat, muss man u.U. noch der zugehörigen Library mitteilen, auf welchem ADC-Kanal nun gemessen werden kann. Die Anpassung muss (z.B. für die M256) im zugehörigen Configuration Header (Datei RP6M256_MultiIO.h) erfolgen:
// 3V3 Voltage Sensor: #define ADC_MULTIIO_3V3 ADC_9 // At ADC-Mxxx //#define ADC_MULTIIO_3V3 ADC_12 // At ADC-Mxxx //#define ADC_MULTIIO_3V3 ADC_4 // At ADC-M256
Hier ist DIE Definition zu wählen, die zur Jumperstellung passt. Nach dem Neukompilieren der Library wird die Änderung wirksam.
Bei der Auswahl von Portpins für eine bestimmte Funktion der MultiIO ist Folgendes unbedingt zu beachten:
- Man sollte sich vergewissern, dass der gewählte Portpin (bzw. ADC-Kanal) nicht schon für eine andere Funktion benutzt wird.
- Man sollte sicher stellen, dass die gewünschte Funktion (z.B. ein ADC-Kanal) tatsächlich an dem festgelegten Pin des IO- oder ADC-Mxxx Steckers vorhanden ist.
- Es ist zu empfehlen, sich an die Standard-Stellung der Jumper zu halten, weil eine Änderung möglicherweise zu Problemen führt, wenn unterschiedliche Programme die veränderte Library nutzen: Man muss dann gut die Verwendung der Portpins dokumentieren.
So weit das Beispiel der 3,3V-Messung mit der M256. Auf die selbe Weise kann man die weiteren Ein-/Ausgabe-Funktionen der MultiIO mit ihren Jumperstellungen auch für die anderen Prozessorsysteme (M128, M32) nachvollziehen.
Siehe auch
- RP6
- RP6 - Programmierung
- RP6v2
- RP6 Sensor Board und Xtra Module
- RP6 Kamera - Mitmach-Projekt
- RP6 - Morse-Code
- RP6v2 I2C-Portexpander
- RP6v2 USB-RS232-Adapter
- RP6v2 Orientierung
- RP6 Multi IO Projekt
- RP6 ArduIO
- RP6 ArduIO - Software
- CCRP5
- Yeti
- Asuro
- C't-Bot
Weblinks
Umfangreiche Dokumentation, sämtliche Beispielprogramme und Bibliotheken, Daten, Teilelisten und eine Lötanleitung finden sich HIER.
Orientierungs-Mitmach-Projekt von Dirk
Umbau des Snake Vision für RP6
Autoren
--Dirk 10:41, 13. Jan 2015 (CET)
--fabqu 21:30, 9. Mär 2013 (CET)
