Aus RN-Wissen.de
Dirk (Diskussion | Beiträge) K (→SHARP Sensoren) |
Dirk (Diskussion | Beiträge) K (→SHARP Sensoren) |
||
| Zeile 351: | Zeile 351: | ||
====SHARP Sensoren==== | ====SHARP Sensoren==== | ||
| − | Das RP6 Sensor Board verfügt über zwei Stecker SHARP_F (K5) und SHARP_R (K3) zum Anschluß von analogen SHARP Distanz-Sensoren. Folgende SHARP Sensoren sind anschließbar: | + | Das RP6 Sensor Board verfügt über zwei Stecker SHARP_F (K5) und SHARP_R (K3) zum Anschluß von analogen SHARP Infrarot-Distanz-Sensoren. Folgende SHARP Sensoren sind anschließbar: |
* 1. SHARP Distanz-Sensor GP2Y0A41SK0F ([http://www.conrad.de/ce/de/product/504596/Distanz-Sensor-GP2Y0A41SK0F-Sharp-GP2Y0A41SK0F-Messbereiche-Erfassungsbereich-4-30-cm-5-VDC 504596]), Messbereich 4..30cm | * 1. SHARP Distanz-Sensor GP2Y0A41SK0F ([http://www.conrad.de/ce/de/product/504596/Distanz-Sensor-GP2Y0A41SK0F-Sharp-GP2Y0A41SK0F-Messbereiche-Erfassungsbereich-4-30-cm-5-VDC 504596]), Messbereich 4..30cm | ||
* 2. SHARP Distanz-Sensor GP2Y0A21YK0F ([http://www.conrad.de/ce/de/product/504591/Distanz-Sensor-GP2Y0A21YK0F-Sharp-GP2Y0A21YK0F-Messbereiche-Erfassungsbereich-10-80-cm-5-VDC 504591]), Messbereich 10..80cm | * 2. SHARP Distanz-Sensor GP2Y0A21YK0F ([http://www.conrad.de/ce/de/product/504591/Distanz-Sensor-GP2Y0A21YK0F-Sharp-GP2Y0A21YK0F-Messbereiche-Erfassungsbereich-10-80-cm-5-VDC 504591]), Messbereich 10..80cm | ||
Version vom 3. Mai 2014, 12:38 Uhr
Inhaltsverzeichnis
- 1 Was bisher geschah
- 2 Allgemein
- 3 RP6 Sensor Board
- 4 RP6 Xtra Module
- 5 Programmierung
- 6 Erfahrungsberichte
- 7 Siehe auch
- 8 Weblinks
- 9 Autoren
Was bisher geschah
Siehe auch "Was bisher geschah" im RP6v2 und im RP6 Artikel!
Mit, 27. Nov 2013 - Ankündigung einer Sensorerweiterungsplatine, die an die RP6v2-M256-WIFI angeschlossen werden kann
Sam, 22. Feb 2014 - Auslieferungsbeginn des RP6 Sensor Boards, des I2C GPS Empfängers, des 3D Accelerometer Moduls und des Gyro Moduls
Mon, 17. Mär 2014 - Das RP6 Sensor Board ist "nicht lieferbar", neuer Liefertermin angekündigt für den 12.06.2014 (!?)
Die, 25. Mär 2014 - Das RP6 Sensor Board ist wieder "sofort lieferbar"
Allgemein
Dieser Artikel beschreibt die Grundlagen und Programmierung des RP6 Sensor Boards und der RP6 Xtra Module. Dieses Erweiterungs-Modul und die Xtra Module sind Teile des RP6-Systems, das in den RN-Wissen-Artikeln RP6, RP6v2 und RP6 - Programmierung ausführlich beschrieben wird. Ausgegliedert aus den "Haupt-Artikeln" habe ich diese Module, da sie m.E. zu einer neuen "Ära" des RP6-Systems gehören:
Sie wurden nicht mehr von SlyD (Dominik S. Herwald) in Zusammenarbeit mit AREXX entwickelt, sondern nun von JM3 Engeneering (Dipl.-Ing. Jürgen Maisel). Ab Februar 2014 waren dann das RP6 Sensor Board RP6-JM03-61 (1082384) und die drei RP6 Xtra Module I2C GPS Empfänger JM3-GPS (1082385), Gyro Modul JM3-GYRO (1082386) und 3D Accelerometer Modul JM3-3DA (1082387) bei CONRAD lieferbar. Einen ersten Eindruck vom Sensor Board kann man im RoboterNetz und AREXX Support Forum bekommen,- siehe unter Weblinks!
RP6 Sensor Board
Das RP6 Sensor Board gehört zu den "Erweiterungs-Modulen" des RP6-Systems. Andere "Erweiterungs-Module" werden im RP6v2 und im RP6 Artikel beschrieben!
Das RP6 Sensor Board ist das erste "Erweiterungs-Modul", das nicht über einen eigenen Microprozessor verfügt,- es ist also ein passives Modul, das von einem Microprozessor-System (z.B. RP6v2, M32, M128, M256 Wifi) angesteuert werden muss. Es ist ebenfalls das erste Modul, das nicht mehr über die rautenförmige Einbuchtung an der Vorderkante verfügt, sondern komplett rechteckig ist. Damit passt es (montiert auf den Standard-Abstandbolzen) nur auf den vorderen Platinenstapel 1 des RP6.
RP6 Xtra Module
Die RP6 Xtra Module gehören zu den "Zubehör- und Ersatzteilen" des RP6-Systems. Über die weiteren "Zubehör- und Ersatzteile" zum RP6 informiert der entsprechende Abschnitt im RP6v2 und im RP6 Artikel!
Die RP6 Xtra Module unterscheiden sich von den weiteren "Zubehör- und Ersatzteilen" dadurch, dass sie selbstständige Sensor-Platinen sind, die ideal zum RP6-System passen, aber auch mit anderen Microcontroller-Plattformen nutzbar sind.
RP6 Sensor Board
Das RP6 Sensor Board RP6-JM03-61 (1082384) wurde am 27.11.2013 von SlyD angekündigt (siehe Weblink!).
Beschreibung
(Laut Online-Katalog der Firma CONRAD!)
"Das RP6 Sensor Board erweitert die Möglichkeiten aller RP6 Roboter um wesentliche Sensorsignale für Robotik-Anwendungen wie zum Beispiel Dreharten (Yaw-Gyro), Kipp- und Neigewinkel, Kompassrichtung oder aber auch Zeit und Datum. Sie können einfach mit zusätzlichen Sharp Sensoren Abstände genau messen, z.B. für autonome Fahrrouten oder falls der Sensor nach unten gerichtet ist, kann er Treppenstufen oder ähnliches entdecken. Außerdem gibt es die Möglichkeit z.B. die Sharp Sensoren ein- bzw. auszuschalten um Strom zu sparen. Zusätzlich können bis zu 8 Servos angesteuert werden um z.B. einen Roboterarm zu bewegen, der auch mechanisch einfach zu montieren ist. Um die Möglichkeiten des RP6 Sensor Boards komplett ausnutzen zu können, wird ein RP6V2 und RP6Wifi Control Board empfohlen. Die ermöglicht den Roboter über Wifi fernzusteuern oder Sensordaten auf dem Kommandozentrum (PC) z.B Kompassdaten, Temperatur oder Kipp- und Neigewinkel anzeigen zu lassen."
Ausstattung
- Magnetometer- und Accelerometer für Kompassfunktion
- Gyro Sensor (Yaw)
- Sharp-Sensor Schnittstelle
- LED Scheinwerfer
- LED Treiber mit Smart Funktionen
- I²C Bus mit max. 400 kHz Übertragungsrate; Hot Swap I²C-Bus Buffer
- Taster mit LED
- Echtzeit-Uhr RTC
- Backupbatterie (Aufladung während des Betriebs)
- Expansion Port z.B. für GPS-I²C Bus Modul
- 8 Servo-Ausgänge mit eigenem Schaltregler
- 8 PWM Ausgänge
- 5 LED Ausgänge und 2 Digital-I/O Ausgänge.
Lieferumfang
- RP6 Sensor Board
- Montage-Kit
- Kabel
- Anleitung und CD mit Dokumentation.
Blockschaltbild
Technische Daten
| Mikrocontroller: | kein |
| Speicher: | kein |
| Programmierung: | C++ (GCC, u.a.) über externes µC-System |
| Vorhandene Sensoren: | Temperatur, 3D Magnetfeld (Kompass), 3D Beschleunigung, Yaw-Gyro |
| Vorhandene Aktoren: | LED Controller (2 LEDs on board, 5 freie LED Ports), Status LED, Button, Servo (PWM) Controller für 8 Servos und 8 PWM-Kanäle, Real Time Clock (RTC), 2 freie I/O Ports |
| Abmessungen: | (L × B × H) 112 × 90 × 19 mm (B + 5 mm durch Scheinwerfer vorn) |
| Ausführung: | Fertig aufgebautes Erweiterungsmodul |
| Stromversorgung: | 6 AA NiMH Akkus (über die RP6v2 Base) und optionaler Zusatzakku 7,2..10 V für Servo Power |
| Hersteller: | Arexx Niederlande |
Umbau-Optionen
Hier soll beschrieben werden, welche Umbau-Optionen es für das RP6 Sensor Board gibt. Im Text gibt es Verweise auf den Schaltplan des RP6 Sensor Boards (RP6_Sensor_Board_Rev2_Schema.pdf) in der Form: (SP2: Bauteil). Dies bedeutet, dass man das Bauteil auf Blatt 2 des Schaltplans finden kann. Beispielhaft sind im folgenden Text Bestell-Nummern der Firma CONRAD genannt. Natürlich kann man die Teile auch bei anderen Versendern beziehen.
USRBUS- und XBUS-Wannenstecker
Der zweite Wannenstecker für den XBUS (SP3: K14) und USRBUS (SP3: K16) kann nachbestückt werden. Man braucht dazu:
- 2 Wannenstecker 2x5-polig gerade RM 2,54mm (CONRAD 741648)
Die Wannenstecker lötet man auf die eingezeichneten Positionen.
USRBUS-Belegung:
| Stecker-Pin | USRBUS | Sensor Board-Funktion | Verwendung | I/O |
| 1 | Y1 | "ADC0" | SHARP vorn | I |
| 2 | Y2 | "ADC1" | Yaw-Gyro | I |
| 3 | Y3 | PWR | Power für SHARPs, CAM_IF, I2C-MODULE |
O |
| 4 | Y4 | keine | ||
| 5 | Y5 | "ADC4" | SHARP hinten | I |
| 6..14 | Y6..Y14 | keine |
XBUS-Belegung:
| Stecker-Pin | XBUS | M256-Port | M256-Funktion | Sensor Board-Funktion | Jumper | Verwendung | I/O |
| 1 | GND | GND | GND | ||||
| 2 | GND | GND | GND | ||||
| 3 | VDD | VDD | VDD_1 | VDD +5V | |||
| 4 | +UB | +UB | VSS_1 | CAM_IF Power | |||
| 5 | VDD | VDD | VDD_1 | VDD +5V | |||
| 6 | +UB | +UB | VSS_1 | CAM_IF Power | |||
| 7 | INTU | PJ4 | PCINT13 | INTU | keine | I | |
| 8 | INT1 | PJ3 | PCINT12 | INT1 | 3 | LSM303DLHC: INT1 | I |
| 9 | INT3 | PJ5 | PCINT14 | INT3 | 2 5 |
DS1339: SQW/INT_N LSM303DLHC: INT2 |
I I |
| 10 | SCL | PD0 | SCL | SCL_M | I2C-Bus | O | |
| 11 | INT2 | PJ6 | PCINT15 | INT2 | 4 | MAX7311: INT_N | I |
| 12 | SDA | PD1 | SDA | SDA_M | I2C-Bus | I/O | |
| 13 | MRESET | MRESET | MRESET | keine | |||
| 14 | GND | GND | GND |
I/O-Patchfield_1
Dieses 10-polige "Patchfield_1" (SP3: K8), beschriftet mit "I/O" liegt neben dem Wannenstecker I/O und weist dieselbe Belegung auf. Möchte man auf dem Sensor Board Verbindungen über den Wannenstecker I/O zu einer angeschlossenen Microprozessor-Platine herstellen, dann kann man auf das "Patchfield_1" eine Stiftleiste löten. Man braucht dazu:
- 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)
Man trennt von der 2-reihigen Stiftleiste 2x5 Kontakte ab und lötet sie auf die Lötpunkte "I/O". Zuvor muss man u.U. Lötzinnreste aus den Lötpunkten mit Entlötlitze bzw. Entlötsaugpumpe entfernen.
Stecker I/O an M256 IO_PWM/T0/T1:
| Stecker-Pin | M256-Port | M256-Funktion | Sensor Board-Funktion | Verwendung | I/O |
| 1 | PD4 | ICP1 | keine | ||
| 2 | GND | keine | |||
| 3 | PD6 | T1 | keine | ||
| 4 | PB7 | OC0A | keine | ||
| 5 | PG5 | OC0B | keine | ||
| 6 | PD7 | T0 | keine | ||
| 7 | PB5 | OC1A | keine | ||
| 8 | PK5 | ADC13 | keine | ||
| 9 | PB6 | OC1B | "PD5" | I2C Bus Buffer Enable | O |
| 10 | VDD | keine |
ADC-Patchfield_2
Dieses 10-polige "Patchfield_2" (SP3: K9), beschriftet mit "ADC" liegt neben dem Wannenstecker ADC und weist dieselbe Belegung auf. Möchte man auf dem Sensor Board Verbindungen über den Wannenstecker ADC zu einer angeschlossenen Microprozessor-Platine herstellen, dann kann man auf das "Patchfield_2" eine Stiftleiste löten. Man braucht dazu:
- 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)
Man trennt von der 2-reihigen Stiftleiste 2x5 Kontakte ab und lötet sie auf die Lötpunkte "ADC".
Stecker ADC an M256 ADC_IO2/CMP:
| Stecker-Pin | M256-Port | M256-Funktion | Sensor Board-Funktion | Jumper | Verwendung | I/O |
| 1 | PK0 | ADC8 | keine | |||
| 2 | GND | keine | ||||
| 3 | PK1 | ADC9 | "ADC4" | 6 | SHARP hinten | I |
| 4 | PE3 | OC3A/AIN1 | keine | |||
| 5 | PK2 | ADC10 | keine | |||
| 6 | GND | keine | ||||
| 7 | PK3 | ADC11 | keine | |||
| 8 | PE2 | XCK0/AIN0 | keine | |||
| 9 | PK4 | ADC12 | keine | |||
| 10 | VDD | keine |
Anschluss-Optionen
Hier soll beschrieben werden, welche Anschluss-Optionen es für das RP6 Sensor Board gibt. In erster Linie geht es dabei um die Belegung der vorhandenen Stecker.
I2C-MODULE
I2C-MODULE-Belegung:
| Stecker-Pin | Sensor Board -Funktion (M1) |
Verwendung | I/O | I2C GPS Empfänger -Funktion (K1) | |
| 1 | VCC | +5V | VCC_1 | ||
| 2 | SCLOUT | Buffered I2C-Bus | O | SCL/SCK | |
| 3 | SDAOUT | Buffered I2C-Bus | I/O | SDA/MOSI | |
| 4 | NC | keine | |||
| 5 | I/O1 | Status LED | O | ||
| 6 | I/O2 | Taster | I | ||
| 7 | NC | keine | |||
| 8 | GND | GND (geschaltet!) | GND | ||
| 9 | GND | GND (geschaltet!) | GND |
CAM_IF
CAM_IF-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K28) |
Verwendung | I/O |
| 1 | GND | GND (geschaltet!) | |
| 2 | VSS_1 | +7,2..10V (RP6: +UB) | |
| 3 | SCLOUT | Buffered I2C-Bus | O |
| 4 | SDAOUT | Buffered I2C-Bus | I/O |
| 5 | SCL_M | I2C-Bus | O |
| 6 | SDA_M | I2C-Bus | I/O |
IO_EXT
IO_EXT-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K19) |
Verwendung | I/O |
| 1 | MAX7311: I/O1.2 | freier I/O Port | I/O |
| 2 | GND | ||
| 3 | MAX7311: I/O1.1 | freier I/O Port | I/O |
| 4 | GND |
PWM_OUT
PWM_OUT-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K12) |
Verwendung | I/O |
| 1 | PCA9685: PWM0 | freier PWM Port | O |
| 2 | PCA9685: PWM1 | freier PWM Port | O |
| 3 | PCA9685: PWM2 | freier PWM Port | O |
| 4 | PCA9685: PWM3 | freier PWM Port | O |
| 5 | PCA9685: PWM4 | freier PWM Port | O |
| 6 | PCA9685: PWM5 | freier PWM Port | O |
| 7 | PCA9685: PWM6 | freier PWM Port | O |
| 8 | PCA9685: PWM7 | freier PWM Port | O |
TP1
Am Testpunkt TP1 (SP4: TP1) kann man das Signal SQW/INT_N der RTC (DS1339) prüfen/messen.
LEDs
Rücklichter
Rücklichter-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K10) |
Verwendung | I/O |
| 1 | TCA6507: P3 | Rücklicht rechts | O |
| 2 | TCA6507: P4 | Rücklicht links | O |
Display-Hintergrundbeleuchtung
Display-Hintergrundbeleuchtung-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K7) |
Verwendung | I/O |
| 1 | TCA6507: P0 | Display-Hintergrundbeleuchtung | O |
| 2 | VDD_1 | +5V |
Freie Ausgänge
Freier Ausgang 1-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K4) |
Verwendung | I/O |
| 1 | TCA6507: P1 | freier LED Port | O |
| 2 | VDD_1 | +5V |
Freier Ausgang 2-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K2) |
Verwendung | I/O |
| 1 | TCA6507: P2 | freier LED Port | O |
| 2 | VDD_1 | +5V |
SHARP Sensoren
Das RP6 Sensor Board verfügt über zwei Stecker SHARP_F (K5) und SHARP_R (K3) zum Anschluß von analogen SHARP Infrarot-Distanz-Sensoren. Folgende SHARP Sensoren sind anschließbar:
- 1. SHARP Distanz-Sensor GP2Y0A41SK0F (504596), Messbereich 4..30cm
- 2. SHARP Distanz-Sensor GP2Y0A21YK0F (504591), Messbereich 10..80cm
- 3. SHARP Distanz-Sensor GP2Y0A02YK (185364), Messbereich 20..150cm
- 4. SHARP Distanz-Sensor GP2Y0A710K0F (504597), Messbereich 100..550cm
Die Auswahl der geeigneten SHARP Sensoren hängt von der gewünschten Funktion ab: Zur Erkundung großer Räume eignet sich insbesondere der Typ 4 mit einer hohen Reichweite von 5,5m. Er ist aber ungeeignet, die Distanz von Objekten zu erkennen, die sich näher als 1m vor dem Sensor befinden. Ein guter Kompromiss zwischen Nahbereichsmessung und "Fernsicht" sind die Typen 2 (10..80cm) und 3 (20..150cm). Will man den Distanz-Sensor vor allem zur Vermeidung eins Sturzes von einer Treppe benutzen (Tiefenerkennung), dann eignet sich insbesondere für den vorderen Sensor (SHARP_F) der Typ 1, da er von 4 bis 30cm messen kann.
Für die Verbindung zwischen den beiden 3-poligen SHARP Steckern auf dem Sensor Board und den SHARP-Sensoren braucht man auf der Sensor Board Seite folgende Stecker-/buchsenteile:
Vorn
Vorn-Belegung:
| Stecker-Pin | RP6v2-Port | RP6v2-Funktion | Sensor Board -Funktion (K5) |
Verwendung | I/O |
| 1 | PA0 | ADC0 | "ADC0" | SHARP vorn | I |
| 2 | VDD_1 | +5V | |||
| 3 | GND | GND (geschaltet!) |
Hinten
Hinten-Belegung:
| Stecker-Pin | M256-Port | M256-Funktion | Sensor Board -Funktion (K3) |
Verwendung | I/O |
| 1 | PK1 | ADC9 | "ADC4" | SHARP hinten | I |
| 2 | VDD_1 | +5V | |||
| 3 | GND | GND (geschaltet!) |
Servos
Power
An den SERVO_PWR Stecker (K1) muss ein Akku zur Stromversorgung der mit dem Sensor Board verbundenen Servos angeschlossen werden. Der Stecker hat folgende Belegung:
SERVO_PWR-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K1) |
Verwendung |
| 1 | GND_POWER | Servo Power GND |
| 2 | VSS_POWER | Servo Power Eingang +7,2..10V |
Der Akku zur Versorgung der Servos muss eine Nennspannung von 7,2..10V aufweisen. Der Schaltregler auf dem Sensor Board kann bei einer 5,0V Servo-Spannung Ströme bis 3,0A (Spitzen bis 5,0A) liefern. Die Polung des Akkus darf am Stecker SERVO_PWR nicht vertauscht werden. Siehe den oberen Teil der nebenstehende Abbildung! Ratsam ist es, eine Sicherung 3,15A in die Plusleitung zwischen Akku und SERVO_PWR Stecker zu legen.
Wenn man keinen separaten Akku zur Versorgung der Servos nehmen will, kann man auch +UB der RP6v2 Base dafür nutzen. An +UB liegt die Akkuspannung der Base (Nennspannung 7,2V) über eine Sicherung F1 3,15A (RP6v2) bzw. 2,5A (RP6) auf dem Mainboard an. +UB kann man z.B. am EXT Anschluß der Base entnehmen. Die nebenstehende Abbildung zeigt im unteren Teil, an welchen Pins man dort +UB kontaktieren kann!
Dabei ist zu beachten, dass die Sicherung F1 auf dem RP6(v2) Mainboard den nutzbaren Strom für die Servos deutlich begrenzt: Durch diese Sicherung fließt ja nicht nur der Servo-Strom, sondern auch der Strom für den RP6 und alle darauf montierten Zusatzplatinen. Wenn der RP6 also herumfährt und z.B. noch die IR-Sendedioden nutzt, bleibt kaum noch Stromreserve für die Servos auf dem Sensor Board übrig. Das bedeutet: Man sollte unbedingt einen separaten Akku für die Servos auf dem Sensor Board benutzen!
SERVO_PWR-EXT Verbindung
Für die Verbindung zwischen dem 2-poligen SERVO_PWR Stecker auf dem Sensor Board und dem 8-poligen EXT Stecker auf dem RP6v2 Mainboard braucht man folgende Stecker-/buchsenteile:
- JST XH Gehäuse, 2-polig, RM 2,5mm (740549)
- JST XH Gehäuse, 8-polig, RM 2,5mm (740928)
- 10x JST XH Crimpkontakt f. RM 2,5mm (741112)
Mit einer 2-adrigen Litze, die beidseits mit den Crimpkontakten verquetscht (oder verlötet) wird, verbindet man dann den SERVO_PWM Stecker mit dem EXT Stecker, wie in der Abbildung oben gezeigt. Achtung: Plus (+) und Minus (-) nicht vertauschen!
Ein passendes Kabel soll zukünftig (*) mitgeliefert werden.
Auf dem RP6 Mainboard ist der 8-polige EXT Stecker noch nicht bestückt. Wenn man ihn nachrüsten will, muss man noch bestellen:
- JST XH Stiftleiste gerade, 8-polig, RM 2,5mm (740105)
Die Stiftleiste wird auf das RP6 Mainboard gelötet, dabei weisen die beiden Einkerbungen an einer Längsseite der Stiftleiste nach hinten zum Rand des Mainboards.
Auch diese Stiftleiste soll zukünftig (*) zum Lieferumfang gehören.
Zu *) Meinem RP6 Sensor Board (bestellt am 1.3.2014) lag das konfektionierte Kabel und die Stiftleiste noch nicht bei!
Stecker
An die acht 3-poligen Stiftleisten SERVO_1 bis SERVO_8 können Servos angeschlossen werden. Direkt aufstecken kann man Servo-Stecker mit der Belegung von CONRAD, Graupner/JR und Robbe/Futaba (Versorgungsspannung Plus am mittleren Pin).
SERVO_1..8-Belegung:
| Stecker-Pin | Sensor Board -Funktion (K6,11,29..34) |
Verwendung | I/O |
| 1 | PCA9685: PWM15..8 | Servo Signal | O |
| 2 | Servo_PWR | Servo Power +5V (geschaltet!) | |
| 3 | GND_POWER | Servo Power GND |
Roboterarm RA2-HOBBY
Der Arexx Roboterarm RA2-HOBBY bzw. RA2-MINI (191534) kann auf das RP6 Sensor Board montiert werden.
RP6 Xtra Module
Von den AREXX RP6 Xtra Modulen sind bei CONRAD für den RP6v2 erhältlich:
- Arexx I2C GPS Empfänger JM3-GPS (1082385)
- Arexx Gyro Modul JM3-GYRO (1082386)
- Arexx 3D Accelerometer Modul JM3-3DA (1082387)
I2C GPS Empfänger JM3-GPS
(Laut Online-Katalog der Firma CONRAD!)
"Universelles GPS-I²C-Bus Modul für RP6 Roboter. Der GPS Empfänger stellt die NMEA-0183 Datensätze GGA, RMC, GSA und VTG in den veschiedenen Registern zur Verfügung. Damit ist eine einfache Weiterverarbeitung möglich."
Beschreibung
Der I2C GPS Empfänger ist eine kleine (38 x 23 mm) Zusatzplatine (RP6 Xtra Modul), die direkt auf das RP6 Sensor Board aufgesteckt werden kann. Sie kann aber auch überall eingesetzt werden, wo eine stabilisierte 5V Versorgungsspannung und ein 5V-I2C-Bus bis 400 kHz zur Verfügung steht.
Die Platine ist bestückt mit dem "Fastrax GPS antenna module" UC530. Als Backup-Batterie dient eine wieder aufladbare Lithium-Batterie (MS621FE). Zur Kommunikation zwischen dem UC530 und dem I2C-Bus ist ein ATtiny1634 an Bord, der auch die Auswertung des NMEA Protokolls übernimmt. Die 3,3 V Versorgungsspannung wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt. Über den I2C-Bus können folgende Daten der NMEA-0183 Datensätze GGA, RMC, GSA und VTG auf einfache Weise gelesen werden:
- Geograf. Breite
- Geograf. Länge
- GPS Status
- HDOP
- Satellitenzahl
- Kurs
- Geschwindigkeit
- Höhe
- UTC-Zeit
- Datum
- PDOP
- VDOP
- Satellit 1..12
Der ATtiny1634 ist natürlich vorprogrammiert, kann aber über eine vorbereitete 6-polige ISP-Schnittstelle auf der Platine auch umprogrammiert werden.
Lieferumfang
- I2C GPS Empfänger
- CD mit Anleitung und Software-Beispielen.
Technische Daten
- Versorgungsspannung: 5,0 V +/- 5%
- Stromverbrauch: <= 20 mA (typ.)
- I2C-Bus Geschwindigkeit: <= 400 kHz
- GPS Antenne: eingebaut
- Backup Batterie: Lithium, im Betrieb wieder aufladbar
- Temperatur Bereich: -10 bis 65 °C
- Form Faktor: 38 x 23 mm
- Gewicht: ca. 6 g
- Empfänger Spezifikationen (UC530):
- Empfänger: GPS L1 C/A-Code, SPS
- Kanäle: 66/22 (Suche/Track)
- Tracking Empfindlichkeit: -165 dBm typ.
- Navigations Empfindlichkeit: -148 dbm typ (Kaltstart)
- Update Rate: 1 Sek.
- Positions Genauigkeit:
- 3,0m (67%) typ. horizontal
- 5,0m (67%) typ. vertikal
- 0,02 m/s (50%) typ. Geschwindigkeit
- Zeit bis zum ersten Fix: typ. 31 Sek.
- Protokoll: NMEA-0183 Rev. 3.01
Anschluss-Schema
Belegung des 9-poligen Anschluss-Steckers (K1) siehe in dieser Tabelle!
Umbau-Optionen
Wenn man den ATtiny1634 selbst programmieren will, muss man Stiftleisten auf die Platine löten.
Man braucht dazu:
- 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)
Man trennt von der 2-reihigen Stiftleiste 2x3 Kontakte ab und lötet sie auf die Punkte "ISP6". Mit einem ISP-Programmer kann dann eine neue Firmware geflasht werden. Die aktuelle Firmware des ATtiny1634 (Version 1.2) wird als .HEX mitgeliefert, leider ohne Source-Code.
Vorsicht: Dabei kann der Prozessor im schlimmsten Fall nicht mehr ansprechbar sein oder der GPS Empfänger UC530 beschädigt werden. Man sollte diesen Umbau und die ISP-Programmierung nur machen, wenn man genau weiß, was man macht!
Gyro Modul JM3-GYRO
(Laut Online-Katalog der Firma CONRAD!)
"Das universelle Gyro-Sensor Modul für RP6, RP6 V2, RP6 ATMEGA32 und das RP6 V2 Control M256 Wifi-Kit ist ideal zur Erkennung der Blockierung des Roboters in seiner Drehbewegung, sowie der Stabilisierung der Geradeausfahrt."
Beschreibung
Das Gyro Modul ist eine kleine (19 x 17 mm) Zusatzplatine (RP6 Xtra Modul), die im RP6-System genutzt werden kann. Es gibt zwar keinen direkt passenden "Steckplatz" auf einer der Microcontroller-Plattformen (RP6v2, M32, M128, M256 Wifi), jedoch ist der Anschluß einfach: Benötigt werden +5V und GND, sowie ein freier ADC-Kanal, der bis 5V messen kann. Damit kann das Gyro Modul sehr universell eingesetzt werden. Es misst die Drehrate um die Z-Achse (Hochachse oder Yaw).
Die Platine ist bestückt mit dem analogen Gyro-Sensor LY330ALH, dessen Ausgangsspannung durch zwei OP-Amps (LM321, LMP7731MF) in Ruhe bei 2,5 V liegt. Höhere Ausgangsspannungen (bis 5 V) bedeuten eine Drehgeschwindigkeit nach LINKS, niedrigere (bis 0 V) eine Drehgeschwindigkeit nach RECHTS. Die 3,3 V Versorgungsspannung des LY330ALH wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt.
Lieferumfang
- Gyro Modul
- CD mit Anleitung und Software-Beispielen.
Technische Daten
- VDD: 5,0 V +/- 5%
- Vout(0): 2,5 V +/- 2% (ohne Bewegung)
- Vout: Vout(0) + 10mV / dps +/- 2%
- Messbereich: ca. 200 dps max.
Anschluss-Schema
Umbau-Optionen
Am Gyro Modul gibt es nichts umzubauen.
3D Accelerometer Modul JM3-3DA
(Laut Online-Katalog der Firma CONRAD!)
"Universeller 3-Achsen Beschleunigungssensor für RP6, RP6 V2, RP6 ATMEGA32 und das RP6 V2 Control M256 WIFI-Kit. Ideal zur Winkelmessung (Neigung-, Kipp-Winkel) und vieles mehr."
Beschreibung
Das 3D Accelerometer Modul ist eine kleine (15 x 10 mm) Zusatzplatine (RP6 Xtra Modul), die direkt auf den EEPROM-Sockel (IC5) der Zusatzplatine RP6 CONTROL M32 gesteckt und über den SPI-Bus angesteuert werden kann. Das Accelerometer Modul kann aber auch mit allen Microcontroller-Plattformen betrieben werden, die eine stabilisierte 5V-Versorgungsspannung und einen SPI- oder I2C-Bus bieten.
Die Platine ist bestückt mit dem 3D Beschleunigungs-Sensor LIS302DLH. Die 3,3 V Versorgungsspannung des LIS302DLH wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt.
Lieferumfang
- 3D Accelerometer Modul
- CD mit Anleitung und Software-Beispielen.
Technische Daten
- VDD: 5,0 V +/- 5%
- Messbereiche: +-2 g, +-4 g, +-8 g
- Datenausgabe: 16 Bit
- Schnittstellen: I2C, SPI
Anschluss-Schema
Umbau-Optionen
Will man das Modul auf der M32 einsetzen, muss man auf dem 3D Accelerometer Modul die Widerstände R2 und R3 (falls vorhanden!) auslöten (siehe Anleitung zum Modul, Seiten 4/5!) und einen IC-Sockel an Position IC5 und einen Kondensator an Position C19 auf der M32 einlöten, siehe hier!
Die Abbildung rechts zeigt das 3D Accelerometer Modul auf der M32.
Programmierung
In diesem Abschnitt geht es um die Programmierung der Sensoren und Aktoren auf dem RP6 Sensor Board und auf den Xtra Modulen. Die hier veröffentlichte Software baut NICHT auf die mitgelieferten C++ Software-Beispiele auf der "RP6 Robot System Xtra Modules CD-ROM" auf, sondern basiert auf Eigenentwicklungen in GCC.
RP6 Sensor Board
Das RP6 Sensor Board RP6-JM03-61 verfügt nicht über einen eigenen Microprozessor. Daher geht es hier eigentlich nicht um die Programmierung des Sensor Boards, sondern um die Programmierung der Microcontroller-Plattform, an die das Sensor Board angeschlossen ist. In der Regel wird das die RP6 M256 WiFi Zusatzplatine sein, aber auch die RP6v2 Base steuert (als I2C-Slave) in der vorgesehenen Konfiguration gewisse Funktionen des Sensor Boards an. Prinzipiell kann das Sensor Board aber auch an der RP6 CONTROL M32 (passende Library auf Anfrage beim Autor!) oder der RP6 CCPRO M128 Zusatzplatine betrieben werden.
Dokumentation
Die Dokumentation zum RP6 Sensor Board befindet sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM".
Manuals
Das Handbuch kann auch hier eingesehen werden.
ACHTUNG: Das Manual enthält aktuell noch teils irreführende Beschreibungen!
Schematics
Datasheets
Demo-Programmme
Demo-Programme zum RP6 Sensor Board befinden sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM".
RP6 M256 WiFi: Sensor Board Library
Die nachfolgende RP6M256_Sensor_BoardLib (GCC) dient zur Ansteuerung des Sensor Boards durch die RP6M256 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 eine Definition:
- USE_PD5 -> Die Sensor Board Funktion "PD5" steuert den TCA4311A Enable Pin (EN).
Im nachfolgenden Listing ist die Definition USE_PD5 NICHT aktiv (d.h. der TCA4311A Enable Pin (EN) wird vom MAX7311 I/O 1.0 Pin angesteuert!). Dies muss an die eigene Hardware angepaßt werden. Wenn "PD5" den TCA4311A Enable Pin (EN) steuern soll, muss die Zeile mit "USE_PD5" "aktiviert" werden. Dazu entfernt man "//" am Zeilenanfang:
#define USE_PD5
Bevor man Beschleunigungs-, Magnetfeldsensor und Gyroskop nutzen kann, muss man sie kalibrieren:
- Accelerometer:
- 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!
- Magnetometer:
- 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.
- Gyroskop:
- In Ruhelage des RP6 wird ein ADC-Wert von ca. 511 angezeigt, wenn die Definition ADCVAL_ZERORATE_LEVEL im Configuration Header (RP6M256_Sensor_Board.h) vor der ersten Kompilierung zunächst auf 0 gesetzt wurde. Die Kalibrierung des Gyroskops ist abgeschlossen, nachdem man den in Ruhelage angezeigten ADC-Wert als ADCVAL_ZERORATE_LEVEL eingetragen hat.
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 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_Sensor_Board.h) muss die Demo neu kompiliert werden. Vorher sollten die Compiler-Hilfsdateien der Sensor Board-Library (Dateiendungen .st und .o) im Programmverzeichnis gelöscht werden.
- Getestet wird das Ergebnis der Kalibrierung, indem die Ausgabewerte (Temperatur, 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_Sensor_Board.h
/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 M256 WIFI * ---------------------------------------------------------------------------- * ----------------------------- [c]2014 - Dirk ------------------------------- * **************************************************************************** * File: RP6M256_Sensor_Board.h * Version: 1.0 * Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz * Author(s): Dirk * **************************************************************************** * Description: * Configuration header file for new RP6 Sensor Board library. * The RP6 Sensor Board (CONRAD 1082384) is connected to the RP6M256 with the * two plugs: * Sensor Board <---> RP6M256 * -------------------------------- * I/O (K17) <---> IO_PWM/T0/T1 * ADC (K18) <---> ADC_IO2/CMP * The RP6 Sensor Board function "PD5" (IC6: EN) is now connected to RP6M256 * OC1B (PB6) and the RP6 Sensor Board function "ADC4" (SHARP_R) to RP6M256 * ADC9 (PK1). * On the RP6 Sensor Board the functions "ADC0", "ADC1", "PWR" and "ADC4" are * connected to the USRBUS. The USRBUS may be wired on the RP6 mainboard like * this: * Function : Sensor Board - USRBUS: * <---> RP6 Mainboard * -------------------------------------------------------- * SHARP_F : "ADC0" - Pin 1 <---> ADC0: Pin 3 * GYRO : "ADC1" - Pin 2 <---> ADC1: Pin 3 * PWRON ² : "PWR" - Pin 3 <---> PWR * SHARP_R : "ADC4" - Pin 5 <---> Not connected! * At *) It is also possible not to use the USRBUS, but to connect the RP6 * Sensor Board directly to the RP6 mainboard with 3 single cables. * At ²) See RP6 manual "4.6.8. Stromsparfunktionen"! * * **************************************************************************** */ #ifndef RP6M256_SENSOR_BOARD_H #define RP6M256_SENSOR_BOARD_H /*****************************************************************************/ // RP6 Sensor Board hardwired components: // - I2C I/O Port Expander (MAX7311) // - I2C Real Time Clock (RTC DS1339) // - I2C Temperature Sensor (LM75B) // - I2C Servo Controller (PCA9685) // - I2C LED Driver (TCA6507) // - I2C 3D Magnetometer & Accelerometer (LSM303DLHC) // - Analog Yaw-Rate Gyroscope (LY330ALH) // - Hot Swappable 2-Wire Bus Buffer (TCA4311A) // - Button // - Status LED /*****************************************************************************/ // I2C I/O Port Expander (MAX7311): #define I2C_SENSOR_PORTEXP_ADR 0x40 // Default // Interrupt I/O portpin definitions (RP6M256): #define IO_SENSOR_INT1_IN INT1_PI12 // PCINT12 PJ3 XBUS Pin 8 #define IO_SENSOR_INT1_DDR DDRJ #define IO_SENSOR_INT1_PIN PINJ #define IO_SENSOR_INT1_PORT PORTJ #define IO_SENSOR_INT2_IN INT2_PI15 // PCINT15 PJ6 XBUS Pin 11 #define IO_SENSOR_INT2_DDR DDRJ #define IO_SENSOR_INT2_PIN PINJ #define IO_SENSOR_INT2_PORT PORTJ #define IO_SENSOR_INT3_IN INT3_PI14 // PCINT14 PJ5 XBUS Pin 9 #define IO_SENSOR_INT3_DDR DDRJ #define IO_SENSOR_INT3_PIN PINJ #define IO_SENSOR_INT3_PORT PORTJ #define IO_SENSOR_INTU_IN INTU_PI13 // PCINT13 PJ4 XBUS Pin 7 #define IO_SENSOR_INTU_DDR DDRJ #define IO_SENSOR_INTU_PIN PINJ #define IO_SENSOR_INTU_PORT PORTJ // I/O PORT pin numbers: #define PIN7 7 #define PIN6 6 #define PIN5 5 #define PIN4 4 #define PIN3 3 #define PIN2 2 #define PIN1 1 #define PIN0 0 /*****************************************************************************/ // I2C Real Time Clock (RTC DS1339): #define I2C_SENSOR_RTC_ADR 0xd0 // Default /*****************************************************************************/ // I2C Temperature Sensor (LM75B): #define I2C_SENSOR_TEMP_ADR 0x90 // Default /*****************************************************************************/ // I2C Servo Controller (PCA9685): #define I2C_SENSOR_SERVO_ADR 0x88 // Default // Servo and PWM channels: #define CH_SERVO_1 16 // At K6: 1 "SERVO_1" #define CH_SERVO_2 15 // At K11: 1 "SERVO_2" #define CH_SERVO_3 13 // At K29: 1 "SERVO_3" #define CH_SERVO_4 14 // At K30: 1 "SERVO_4" #define CH_SERVO_5 11 // At K31: 1 "SERVO_5" #define CH_SERVO_6 12 // At K32: 1 "SERVO_6" #define CH_SERVO_7 9 // At K33: 1 "SERVO_7" #define CH_SERVO_8 10 // At K34: 1 "SERVO_8" #define CH_PWM_OUT_1 1 // At K12: 1 "PWM_OUT" #define CH_PWM_OUT_2 2 // At K12: 2 #define CH_PWM_OUT_3 3 // At K12: 3 #define CH_PWM_OUT_4 4 // At K12: 4 #define CH_PWM_OUT_5 5 // At K12: 5 #define CH_PWM_OUT_6 6 // At K12: 6 #define CH_PWM_OUT_7 7 // At K12: 7 #define CH_PWM_OUT_8 8 // At K12: 8 // 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 LED Driver (TCA6507): #define I2C_SENSOR_LEDDRIVER_ADR 0x8a // Default // LED numbers: #define LED0 0 // At K7 #define DISPLAY_BACKLIGHT 0 // Display background light #define LED1 1 // At K4 #define LED2 2 // At K2 #define LED3 3 // At K10: 1 #define BACKLIGHT_R 3 // Right backlight #define LED4 4 // At K10: 2 #define BACKLIGHT_L 4 // Left backlight #define LED5 5 // White LED (D3) #define HEADLIGHT_R 5 // Right headlight #define LED6 6 // White LED (D1) #define HEADLIGHT_L 6 // Left headlight /*****************************************************************************/ // I2C 3D Magnetometer & Accelerometer (LSM303DLHC): // LSM303DLHC accelerometer: #define I2C_SENSOR_LSM303DLHC_A_ADR 0x32 // Default // LSM303DLHC accelerometer calibration data: #define MAX_X_A 1037 // Max. X-axis value #define MIN_X_A -1030 // Min. X-axis value #define MAX_Y_A 1030 // Max. Y-axis value #define MIN_Y_A -1019 // Min. Y-axis value #define MAX_Z_A 1062 // Max. Z-axis value #define MIN_Z_A -1056 // Min. Z-axis value #define OFFSET_PITCH_A 0.0 // Offset Pitch [°] #define OFFSET_ROLL_A 0.0 // Offset Roll [°] // LSM303DLHC magnetometer: #define I2C_SENSOR_LSM303DLHC_M_ADR 0x3c // Default // LSM303DLHC magnetometer calibration data: #define MAX_X_M -111.0 // Max. X-axis value #define MIN_X_M -634.0 // Min. X-axis value #define OFFSET_X_M 0 // Hard iron X-axis offset #define MAX_Y_M 297.0 // Max. Y-axis value #define MIN_Y_M -215.0 // Min. Y-axis value #define OFFSET_Y_M 0 // Hard iron Y-axis offset #define MAX_Z_M 183.0 // Max. Z-axis value #define MIN_Z_M -214.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] /*****************************************************************************/ // Analog Yaw-Rate Gyroscope (LY330ALH): // LY330ALH gyroscope: #define ADC_SENSOR_LY330ALH ADC_1 // At USRBUS: 2 (RP6BASE) // LY330ALH gyroscope calibration data: // (Higher values [505..1023] mean an angular velocity while rotating LEFT // and lower values [503..0] while rotating RIGHT!) #define ADCVAL_MINRATE 0 // Min. rate value #define ADCVAL_ZERORATE_LEVEL 504 // Zero rate level #define ADCVAL_MAXRATE 1023 // Max. rate value /*****************************************************************************/ // Hot Swappable 2-Wire Bus Buffer (TCA4311A): // Define USE_PD5, if you want to control the TCA4311A enable pin (EN) with // the RP6 Sensor Board function "PD5". If USE_PD5 is NOT defined (default), // the TCA4311A EN pin is controlled by the MAX7311 I/O 1.0 pin. // --------------- //#define USE_PD5 // --------------- #define IO_SENSOR_TCA4311A_EN_IN OC1B_PI6 // At I/O: 9 (RP6M256) #define IO_SENSOR_TCA4311A_EN_DDR DDRB #define IO_SENSOR_TCA4311A_EN_PIN PINB #define IO_SENSOR_TCA4311A_EN_PORT PORTB /*****************************************************************************/ // Button: // (Button is connected to I/O 1.7 of the MAX7311!) /*****************************************************************************/ // Status LED: // (Status LED is connected to I/O 1.6 of the MAX7311!) /*****************************************************************************/ // Other ADC channel definitions: // (Depending on connections on the RP6 Sensor Board!) #define ADC_SENSOR_SHARP_F ADC_0 // At USRBUS: 1 (RP6BASE) #define ADC_SENSOR_SHARP_R ADC_9 // At ADC: 3 (RP6M256) /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Header
Datei: RP6M256_Sensor_BoardLib.h
/* ****************************************************************************
* _______________________
* \| RP6 ROBOT SYSTEM |/
* \_-_-_-_-_-_-_-_-_-_/ >>> RP6 M256 WIFI
* ----------------------------------------------------------------------------
* ----------------------------- [c]2014 - Dirk -------------------------------
* ****************************************************************************
* File: RP6M256_Sensor_BoardLib.h
* Version: 1.0
* Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
* Author(s): Dirk
* ****************************************************************************
* Description:
* Header file for new RP6 Sensor Board library.
*
* ****************************************************************************
*/
#ifndef RP6M256_SENSOR_BOARDLIB_H
#define RP6M256_SENSOR_BOARDLIB_H
/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6M256_Sensor_Board.h"
#include <math.h>
/*****************************************************************************/
// RP6M256_I2CMasterLib definitions:
#ifndef I2C_RP6_BASE_ADR
#define I2C_RP6_BASE_ADR 10
#endif
#ifndef I2C_REG_ADC_ADC0_L
#define I2C_REG_ADC_ADC0_L 23
#endif
#ifndef I2C_REG_ADC_ADC1_L
#define I2C_REG_ADC_ADC1_L 25
#endif
#ifndef CMD_POWER_OFF
#define CMD_POWER_OFF 0
#endif
#ifndef CMD_POWER_ON
#define CMD_POWER_ON 1
#endif
/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):
// Registers:
#define MAX7311_INPUT_PORT1 0
#define MAX7311_INPUT_PORT2 1
#define MAX7311_OUTPUT_PORT1 2
#define MAX7311_OUTPUT_PORT2 3
#define MAX7311_PORT1_POL_INV 4
#define MAX7311_PORT2_POL_INV 5
#define MAX7311_PORT1_CONFIG 6
#define MAX7311_PORT2_CONFIG 7
#define MAX7311_TIMEOUT 8
// Port 1 configuration register bitmasks:
#define ST_GYRO (1 << PIN7) // I/O 0.7 (Output)
#define PD_GYRO (1 << PIN6) // I/O 0.6 (Output)
#define PWR_CAM_IF (1 << PIN5) // I/O 0.5 (Output)
#define EN_LEDDRIV (1 << PIN4) // I/O 0.4 (Output)
#define RDY_BUSBUFF (1 << PIN3) // I/O 0.3 (Input)
#define PWR_SHARP_R (1 << PIN2) // I/O 0.2 (Output)
#define PWR_SHARP_F (1 << PIN1) // I/O 0.1 (Output)
#define PWR_I2C (1 << PIN0) // I/O 0.0 (Output)
// Port 2 configuration register bitmasks:
#define IO2_I2C (1 << PIN7) // I/O 1.7 (Input)
#define IO1_I2C (1 << PIN6) // I/O 1.6 (Output)
#define DRDY_MAGACC (1 << PIN5) // I/O 1.5 (Input)
#define PWR_SERVO (1 << PIN4) // I/O 1.4 (Output)
#define OEN_SERVO (1 << PIN3) // I/O 1.3 (Output)
#define IO_K19_1 (1 << PIN2) // I/O 1.2 (Input)
#define IO_K19_3 (1 << PIN1) // I/O 1.1 (Input)
#define EN_BUSBUFF (1 << PIN0) // I/O 1.0 (Output)
// Port 1 & 2 register 16-bit bitmasks:
// (CONFIG: 1 = Input, 0 = Output)
#define MAX7311_CONFIG_DEFAULT 0b1010011000001000
#define MAX7311_CONFIG_USE_PD5 0b1010011100001000
// (OUTPUT: 1 = High/True, 0 = Low/False)
#define MAX7311_OUTPUT_DEFAULT 0b0100000100010000
#define MAX7311_OUTPUT_FULLPOWER 0b0001000100110111
#define MAX7311_OUTPUT_SHUTDOWN 0b0100100011000000
// Safe configuration:
// (Ensures if activated, that I/Os 0.3, 1.5 and 1.7 cannot be configured
// as output with the function MAX7311_write_cfg(). This may protect the
// RP6 Sensor Board from hardware damage.)
// -------------------
#define SAFE_CONFIG
// -------------------
typedef union {
uint8_t byte;
struct {
unsigned int1 :1; // INT1 XBUS Pin 8
unsigned int2 :1; // INT2 XBUS Pin 11
unsigned int3 :1; // INT3 XBUS Pin 9
unsigned intu :1; // INTU XBUS Pin 7
unsigned unused :4;
};
struct {
unsigned magacc1 :1; // INT1: LSM303DLHC INT1
unsigned portexp :1; // INT2: MAX7311 INT_N
unsigned magacc2_rtc :1; // INT3: LSM303DLHC INT2
unsigned noname :5; // OR DS1339 SQW/INT_N
};
} interrupt_RP6SBstatus_t;
extern interrupt_RP6SBstatus_t interrupt_RP6SBstatus;
typedef union {
uint16_t word;
struct {
unsigned pwr_i2c :1; // I/O 0.0 (Output)
unsigned pwr_sharp_f :1; // I/O 0.1 (Output)
unsigned pwr_sharp_r :1; // I/O 0.2 (Output)
unsigned rdy_busbuff :1; // I/O 0.3 (Input)
unsigned en_leddriv :1; // I/O 0.4 (Output)
unsigned pwr_cam_if :1; // I/O 0.5 (Output)
unsigned pd_gyro :1; // I/O 0.6 (Output)
unsigned st_gyro :1; // I/O 0.7 (Output)
unsigned en_busbuff :1; // I/O 1.0 (Output)
unsigned io_k19_3 :1; // I/O 1.1 (Input)
unsigned io_k19_1 :1; // I/O 1.2 (Input)
unsigned oen_servo :1; // I/O 1.3 (Output)
unsigned pwr_servo :1; // I/O 1.4 (Output)
unsigned drdy_magacc :1; // I/O 1.5 (Input)
unsigned io1_i2c :1; // I/O 1.6 (Output)
unsigned io2_i2c :1; // I/O 1.7 (Input)
};
} max7311IOs_t;
extern max7311IOs_t inport;
extern max7311IOs_t outport;
extern uint8_t readyBusbuff;
extern uint8_t pressedButton;
void MAX7311_write_cfg(uint16_t);
void task_checkINTs(void);
void task_RP6SensorBoard(void);
void MAX7311_update(void);
void MAX7311_write(uint16_t);
void MAX7311_init(void);
/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):
// Registers:
#define DS1339_SECONDS 0
#define DS1339_MINUTES 1
#define DS1339_HOURS 2
#define DS1339_DAY 3
#define DS1339_DATE 4
#define DS1339_MONTH_CENTURY 5
#define DS1339_YEAR 6
#define DS1339_ALARM1_SECONDS 7
#define DS1339_ALARM1_MINUTES 8
#define DS1339_ALARM1_HOURS 9
#define DS1339_ALARM1_DAY_DATE 10
#define DS1339_ALARM2_MINUTES 11
#define DS1339_ALARM2_HOURS 12
#define DS1339_ALARM2_DAY_DATE 13
#define DS1339_CONTROL 14
#define DS1339_STATUS 15
#define DS1339_TRICKLE_CHARGER 16
// Control register bitmasks:
#define DS1339_CONTROL_DEFAULT 0 // Power on default
#define DS1339_CONTROL_A1IE 1
#define DS1339_CONTROL_A2IE 2
#define DS1339_CONTROL_INTCN 4
#define DS1339_CONTROL_RS1 8
#define DS1339_CONTROL_RS2 16
#define DS1339_CONTROL_BBSQI 32
#define DS1339_CONTROL_EOSC 128
// Trickle Charger register bitmasks:
#define DS1339_TRICKLE_CHARGER_DEFAULT 0b10101010 // One diode, 2k resistor
#define DS1339_TRICKLE_CHARGER_ROUT0 1
#define DS1339_TRICKLE_CHARGER_ROUT1 2
#define DS1339_TRICKLE_CHARGER_DS0 4
#define DS1339_TRICKLE_CHARGER_DS1 8
#define DS1339_TRICKLE_CHARGER_TCS0 16
#define DS1339_TRICKLE_CHARGER_TCS1 32
#define DS1339_TRICKLE_CHARGER_TCS2 64
#define DS1339_TRICKLE_CHARGER_TCS3 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 DS1339_write_cfg(uint8_t);
void DS1339_init(void);
#define CALC_DST // Time zone will be calculated
void DS1339_read(void);
void DS1339_write(void);
/*****************************************************************************/
// I2C Temperature Sensor (LM75B):
// Registers:
#define LM75B_TEMP 0
#define LM75B_CONF 1
#define LM75B_THYST 2
#define LM75B_TOS 3
// Conf register bitmasks:
#define LM75B_CONF_DEFAULT 0 // Power on default
#define LM75B_CONF_SHUTDOWN 1
#define LM75B_CONF_OS_COMP_INT 2
#define LM75B_CONF_OS_POL 4
#define LM75B_CONF_OS_F_QUE0 8
#define LM75B_CONF_OS_F_QUE1 16
extern double temperature;
void LM75B_write_cfg(uint8_t);
#define LM75B_shutdown() {LM75B_write_cfg(LM75B_CONF_SHUTDOWN);}
#define LM75B_init() {LM75B_write_cfg(LM75B_CONF_DEFAULT);}
extern uint8_t temperature_low;
extern uint8_t temperature_high;
#define getTemperatureHigh() (temperature_high)
#define getTemperatureLow() (temperature_low)
void LM75B_read(void);
double LM75B_calculate(void);
double LM75B_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__);}
uint8_t servo(uint8_t);
void PCA9685_set(uint8_t, uint16_t);
#define setServo(__SERVO__,__POS__) {PCA9685_set(servo(__SERVO__),__POS__);}
void PCA9685_shutdown(void);
void PCA9685_restart(void);
void setServoPower(uint8_t);
/*****************************************************************************/
// I2C LED Driver (TCA6507):
// Registers:
#define TCA6507_SELECT0 0
#define TCA6507_SELECT1 1
#define TCA6507_SELECT2 2
#define TCA6507_FADEON_TIME 3
#define TCA6507_FULLYON_TIME 4
#define TCA6507_FADEOFF_TIME 5
#define TCA6507_1ST_FULLYOFF_TIME 6
#define TCA6507_2ND_FULLYOFF_TIME 7
#define TCA6507_MAXIMUM_INTENSITY 8
#define TCA6507_MASTER_INTENSITY 9
#define TCA6507_INITIALIZATION 10
// Output states:
#define STATE_OFF 0
#define STATE_PWM0 2
#define STATE_PWM1 3
#define STATE_ON 4
#define STATE_MINT 5
#define STATE_BLINK0 6
#define STATE_BLINK1 7
extern uint8_t select[3];
void TCA6507_write_cfg(uint8_t, uint8_t);
void TCA6507_update(void);
void TCA6507_set(uint8_t, uint8_t);
#define setLED(__LED__,__STATE__) {TCA6507_set(__LED__,__STATE__);}
void TCA6507_shutdown(void);
void TCA6507_restart(void);
void TCA6507_init(void);
/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):
// 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)
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_mag;
#endif
extern int16_t headingtc;
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 LSM303DLHC_init(void);
/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):
#define LY330ALH_MODE_NORMAL 0 // Power on default
#define LY330ALH_MODE_SLEEP 1
#define LY330ALH_MODE_SELFTEST 2
#define LY330ALH_MODE_POWERDOWN 3 // Shutdown mode
extern int16_t z_axisg;
void LY330ALH_set_mode(uint8_t);
uint16_t getRP6Base_ADC1(void);
int16_t LY330ALH_measure(void);
/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):
//extern uint8_t readyBusbuff; // See "I2C I/O Port Expander" section
extern uint8_t errorBusbuff;
#define getReadyBusbuff() (readyBusbuff)
uint8_t readReadyBusbuff(void);
void TCA4311A_shutdown(void);
void TCA4311A_reconnect(void);
void setRP6Base_PWR(uint8_t);
void setI2CModulePower(uint8_t);
/*****************************************************************************/
// Button:
//extern uint8_t pressedButton; // See "I2C I/O Port Expander" section
#define getPressedButton() (pressedButton)
uint8_t readButton(void);
/*****************************************************************************/
// Status LED:
void setStatusLED(uint8_t);
/*****************************************************************************/
// RP6 Sensor Board initialisation and shutdown:
void sensor_board_init(void);
void sensor_board_shutdown(void);
/*****************************************************************************/
#endif
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
*
* ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c"
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Library Source
Datei: RP6M256_Sensor_BoardLib.c
/* ****************************************************************************
* _______________________
* \| RP6 ROBOT SYSTEM |/
* \_-_-_-_-_-_-_-_-_-_/ >>> RP6 M256 WIFI
* ----------------------------------------------------------------------------
* ----------------------------- [c]2014 - Dirk -------------------------------
* ****************************************************************************
* File: RP6M256_Sensor_BoardLib.c
* Version: 1.0
* Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
* and a RP6 Sensor Board (CONRAD 1082384)
* Author(s): Dirk
* ****************************************************************************
* Description:
* This is our new Library that contains basic routines and functions for
* accessing the hardwired components of the RP6 Sensor Board (CONRAD 1082384).
* This library assumes the RP6 Sensor Board Revision 2.0 (04.07.13).
*
* There are more sensors and actors, that may be connected to the RP6 Sensor
* Board: Many I2C components including a GPS module (CONRAD 1082385), servo
* motors, SHARP IR distance sensors, LEDs, a camera ...
* This library doesn't contain functions for these sensors/actors, because
* they are not HARDWIRED to the RP6 Sensor Board and may be connected
* OPTIONALLY.
*
* ****************************************************************************
*/
/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED
/*****************************************************************************/
// Includes:
#include "RP6M256_Sensor_BoardLib.h"
/*****************************************************************************/
// Variables:
uint8_t registerBuf[13];
/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):
/**
* Sends the configuration word to a MAX7311.
*
* Input: config -> Config word for control
* registers
*
*/
void MAX7311_write_cfg(uint16_t config)
{
#ifdef SAFE_CONFIG
config |= 0b1010000000001000;
#endif
I2CTWI_transmit3Bytes(I2C_SENSOR_PORTEXP_ADR, MAX7311_PORT1_CONFIG, config, (config>>8));
}
interrupt_RP6SBstatus_t interrupt_RP6SBstatus;
/**
* This function checks all interrupt pins on
* the XBUS (INT1..3, INTU).
* This function must be called VERY frequently
* out of the main loop.
* Bigger delays result in slower reaction to
* interrupt requests of the RP6 Sensor Board.
* Interrupt function table:
* Int. : Function 1 IRQ Function 2 IRQ
* -------------------------------------------------
* INT1 : LSM303DLHC INT1 H
* INT2 : MAX7311 INT_N L
* INT3 : LSM303DLHC INT2 H DS1339 SQW/INT_N L
* INTU : not used
* You can see in the "IRQ" column, which level
* (H/L) means an ACTIVE INTERRUPT signal.
* The interrupt pin levels (H=true/L=false) are
* read into the global interrupt_RP6SBstatus
* variable.
*
* Hints: - This function checks the interrupt
* signals by polling. This is NOT the
* best way to to this job. You will
* have to write your own task, if you
* want to use the Pin Change Interrupt
* function of the ATmega2560
* microcontroller.
* - You cannot use both interrupt sources
* for INT3! So you should not use INT2
* of the LSM303DLHC together with the
* alarm function of the DS1339 (RTC)!
* - Every interrupt signal of the Sensor
* Board may be cut off from the XBUS
* by opening a jumper (Jx):
* INT1 -> J3 (LSM303DLHC INT1)
* INT2 -> J4 (MAX7311 INT_N)
* INT3 -> J2 (DS1339 SQW/INT_N)
* INT3 -> J5 (LSM303DLHC INT2)
*
* Example:
*
* if (!interrupt_RP6SBstatus.portexp)
* writeString_P("==> MAX7311 INT_N!!!");
*
*/
void task_checkINTs(void)
{
if(IO_SENSOR_INT1_PIN & IO_SENSOR_INT1_IN) // XBUS INT1 -> PJ3 (PCINT12)
interrupt_RP6SBstatus.int1 = true;
else
interrupt_RP6SBstatus.int1 = false;
if(IO_SENSOR_INT2_PIN & IO_SENSOR_INT2_IN) // XBUS INT2 -> PJ6 (PCINT15)
interrupt_RP6SBstatus.int2 = true;
else
interrupt_RP6SBstatus.int2 = false;
if(IO_SENSOR_INT3_PIN & IO_SENSOR_INT3_IN) // XBUS INT3 -> PJ5 (PCINT14)
interrupt_RP6SBstatus.int3 = true;
else
interrupt_RP6SBstatus.int3 = false;
// if(IO_SENSOR_INTU_PIN & IO_SENSOR_INTU_IN) // XBUS INTU -> PJ4 (PCINT13)
// interrupt_RP6SBstatus.intu = true;
// else
// interrupt_RP6SBstatus.intu = false;
}
max7311IOs_t inport;
uint8_t readyBusbuff;
uint8_t pressedButton;
/**
* This function checks all I/O ports (I/O 0.0..1.7)
* of the MAX7311 sequentially!
* You must call this function frequently out of
* the main loop.
* The I/O port values (true/false) are read into
* the global inport variable.
* The global variables readyBusbuff (2-Wire Bus
* Buffer: Enable) and pressedButton (Button S1:
* Pressed) contain actual values (true/false)
* after execution of this task.
*
* Examples:
*
* if (inport.rdy_busbuff)
* writeString_P("I/O 0.3: High");
* else writeString_P("I/O 0.3: Low");
* if (!inport.drdy_magacc)
* writeString_P("I/O 1.5: Low");
* if (pressedButton)
* writeString_P("Button pressed!");
*
*/
void task_RP6SensorBoard(void)
{
I2CTWI_transmitByte(I2C_SENSOR_PORTEXP_ADR, MAX7311_INPUT_PORT1);
I2CTWI_readBytes(I2C_SENSOR_PORTEXP_ADR, registerBuf, 2);
inport.word = (registerBuf[1] << 8) + registerBuf[0];
readyBusbuff = inport.rdy_busbuff;
pressedButton = inport.io2_i2c;
pressedButton = !pressedButton;
}
max7311IOs_t outport;
/**
* Updates the I/O ports with current value from
* the global outport variable!
* If one or more port pins were configured as
* output before, they will be set (high) or
* cleared (low).
*
* Example:
*
* outport.word = 0b0000000000001001;
* MAX7311_update();
* // This clears all I/O ports and sets
* // the ports I/O 0.0 and I/O 0.3!
*
* // Other possibility:
* outport.pwr_sharp_f = true;
* MAX7311_update();
* // This sets I/O 0.1 and does not
* // affect any other I/O port!
*
*/
void MAX7311_update(void)
{
I2CTWI_transmit3Bytes(I2C_SENSOR_PORTEXP_ADR, MAX7311_OUTPUT_PORT1, outport.word, (outport.word>>8));
}
/**
* Writes the 16-bit data to a MAX7311.
* The data are output with those I/O ports that
* were configured as output before.
*
* Input: data -> Data word
*
*/
void MAX7311_write(uint16_t data)
{
outport.word = data;
MAX7311_update();
}
/**
* Initializes the I/O Port Expander after power
* on.
*
* Hint: If USE_PD5 is defined in the
* configuration header file, this
* function uses the RP6M256 portpin,
* that is connected to plug I/O: Pin 9
* instead of MAX7311 I/O 1.0!
*
*/
void MAX7311_init(void)
{
#ifdef USE_PD5
MAX7311_write_cfg(MAX7311_CONFIG_USE_PD5);
IO_SENSOR_TCA4311A_EN_DDR |= IO_SENSOR_TCA4311A_EN_IN; // Output
IO_SENSOR_TCA4311A_EN_PORT |= IO_SENSOR_TCA4311A_EN_IN; // High
#else
MAX7311_write_cfg(MAX7311_CONFIG_DEFAULT);
#endif
MAX7311_write(MAX7311_OUTPUT_DEFAULT);
}
/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):
/**
* 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 DS1339.
*
* Input: Config byte for control register
*
*/
void DS1339_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_SENSOR_RTC_ADR, DS1339_CONTROL, config);
}
/**
* Initializes the DS1339 Trickle Charger
* function to the default for the RP6 Sensor
* Board: One diode and 2kOhm resistor.
*
*/
void DS1339_init(void)
{
I2CTWI_transmit2Bytes(I2C_SENSOR_RTC_ADR, DS1339_TRICKLE_CHARGER, DS1339_TRICKLE_CHARGER_DEFAULT);
}
/**
* Reads all data registers of the Real Time Clock (RTC).
* They are stored in the time & date variables defined in
* the library header.
*
*/
void DS1339_read(void)
{
I2CTWI_transmitByte(I2C_SENSOR_RTC_ADR, DS1339_SECONDS);
I2CTWI_readBytes(I2C_SENSOR_RTC_ADR, registerBuf, 7);
rtc_time.second = BCD2DEC(registerBuf[0]);
rtc_time.minute = BCD2DEC(registerBuf[1]);
rtc_time.hour = BCD2DEC(registerBuf[2] & 0x3f);
rtc_date.weekday = (registerBuf[3] & 0x07);
rtc_date.day = BCD2DEC(registerBuf[4]);
rtc_date.month = BCD2DEC(registerBuf[5] & 0x7f);
if(registerBuf[5] & 0x80) rtc_date.year = 100;
else rtc_date.year = 0;
rtc_date.year += BCD2DEC(registerBuf[6]);
rtc_date.year += 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 DS1339_write(void)
{
registerBuf[0] = DS1339_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_SENSOR_RTC_ADR, ®isterBuf[0], 8);
}
/*****************************************************************************/
// I2C Temperature Sensor (LM75B):
/**
* Sends the configuration byte to a LM75B.
*
* Input: Config byte for Conf register
*
*/
void LM75B_write_cfg(uint8_t config)
{
I2CTWI_transmit2Bytes(I2C_SENSOR_TEMP_ADR, LM75B_CONF, config);
}
uint8_t temperature_low;
uint8_t temperature_high;
/**
* Reads the two data registers of the temperature
* sensor's Temp register.
* They are stored in the variables temperature_low
* and _high.
*
*/
void LM75B_read(void)
{
I2CTWI_transmitByte(I2C_SENSOR_TEMP_ADR, LM75B_TEMP);
I2CTWI_readBytes(I2C_SENSOR_TEMP_ADR, registerBuf, 2);
temperature_high = registerBuf[0];
temperature_low = registerBuf[1];
}
double temperature; // Temperature [°C]
/**
* Calculates and returns the temperature value
* by using the data read from the LM75B with
* the function LM75B_read(). The sensor is
* designed for 11 bit measurement and 0.125°C
* resolution.
*
*/
double LM75B_calculate(void)
{
int16_t tempraw = 0;
double temp;
tempraw = (getTemperatureHigh() << 8) | getTemperatureLow();
tempraw >>= 5;
if (tempraw & 1024) // Calculate temperature
tempraw = (tempraw & 1023) - 1024;
else
tempraw = tempraw & 1023;
temp = tempraw * 0.125;
return temp;
}
/**
* Performs a 11 bit measurement and returns the
* temperature [°C]. The sensor has a measuring
* cycle of 100ms. So it makes no sense calling
* this function at a frequency above 10Hz.
*
*/
double LM75B_measure(void)
{
// LM75B_init();
LM75B_read(); // Read data
return (LM75B_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)
{
outport.oen_servo = false; // I/O 1.3: Low
MAX7311_update();
if ((freq < 40) || (freq > 1000)) freq = 50;
I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE2);
uint8_t last_mode = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
last_mode &= ~PCA9685_MODE2_INVRT; // Clear INVRT bit
last_mode |= PCA9685_MODE2_OUTDRV; // Set OUTDRV bit
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE2, last_mode);
I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1);
last_mode = I2CTWI_readByte(I2C_SENSOR_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_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
uint8_t prescale = (uint8_t) (F_PCA9685 / 4096 / freq - 0.5);
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_PRE_SCALE, prescale);
last_mode &= ~PCA9685_MODE1_SLEEP; // Clear SLEEP bit
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, last_mode);
mSleep(1);
last_mode |= PCA9685_MODE1_RESTART; // Clear RESTART bit
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, last_mode);
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_LED8_ON_H, 0x10);
return;
}
/**
* Returns the channel belonging to a servo number.
*
* Input: number -> Servo number [1..8]
*
*/
uint8_t servo(uint8_t number)
{
uint8_t channel;
switch (number) {
case 1 :
channel = CH_SERVO_1; break;
case 2 :
channel = CH_SERVO_2; break;
case 3 :
channel = CH_SERVO_3; break;
case 4 :
channel = CH_SERVO_4; break;
case 5 :
channel = CH_SERVO_5; break;
case 6 :
channel = CH_SERVO_6; break;
case 7 :
channel = CH_SERVO_7; break;
case 8 :
channel = CH_SERVO_8; break;
default : channel = CH_SERVO_1;
}
return channel;
}
/**
* This is the servo position set function.
*
* Input: channel -> Channel number [1..16]
* pos -> Servo position [SERVOx_LT..SERVOx_RT]
*
* Hints: - 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 nearly the same as this function, but the first
* parameter is the servo and NOT the channel number.
*
* Example:
* setServo(2,300);
*
*/
void PCA9685_set(uint8_t channel, uint16_t pos)
{
if ((channel == 0) || (channel > 16))
return;
uint8_t reg = channel * 4 + 4; // Register LEDx_OFF_L
I2CTWI_transmit3Bytes(I2C_SENSOR_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_SENSOR_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0x10);
I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1);
uint8_t mode1 = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
mode1 |= PCA9685_MODE1_SLEEP; // Set SLEEP bit
I2CTWI_transmit2Bytes(I2C_SENSOR_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_SENSOR_SERVO_ADR, PCA9685_MODE1);
uint8_t mode1 = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
if (mode1 & PCA9685_MODE1_RESTART) { // RESTART bit set?
mode1 &= ~PCA9685_MODE1_SLEEP; // Clear SLEEP bit
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
mSleep(1);
mode1 |= PCA9685_MODE1_RESTART; // Clear RESTART bit
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0);
}
}
/**
* With this function you can switch the servo
* power on or off.
*
* 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
* (default) to save energy!
* - The PCA9685 is NOT restarted or put into
* shutdown mode by this function!
*
*/
void setServoPower(uint8_t pwr)
{
if(pwr > 0)
outport.pwr_servo = true; // I/O 1.4: High
else
outport.pwr_servo = false; // I/O 1.4: Low
MAX7311_update();
}
/*****************************************************************************/
// I2C LED Driver (TCA6507):
/**
* Sends the configuration byte to TCA6507's
* internal registers.
*
* Input: reg -> Register address [0..10]
* config -> Config byte for internal
* registers
*
* Hints: - With this function you can write to
* all internal registers of the LED
* driver, see "Registers:" in the
* library header file!
* - Mainly you will use this function
* for setting time (regs 3..7) and
* intensity (regs 8, 9) values.
*
* Example:
* TCA6507_write_cfg(TCA6507_MAXIMUM_INTENSITY,0x77);
*
*/
void TCA6507_write_cfg(uint8_t reg, uint8_t config)
{
if(reg > 10) reg = 0;
I2CTWI_transmit2Bytes(I2C_SENSOR_LEDDRIVER_ADR, reg, config);
}
uint8_t select[3];
/**
* Updates the LED driver select registers (0..2)
* with the global select array!
*
* Hint: - Also have a look at Table 4 in the
* TCA6507 datasheet!
*
*/
void TCA6507_update(void)
{
I2CTWI_transmit4Bytes(I2C_SENSOR_LEDDRIVER_ADR, (TCA6507_SELECT0 | 0x10), select[0], select[1], select[2]);
}
/**
* This is the LED state set function.
*
* Input: led -> LED number [0..6]
* state -> LED state [0..7]
*
* Hints: - Possible driver states [0..7] see
* "Output states:" in the library
* header file!
* - Also have a look at Table 4 in the
* TCA6507 datasheet!
*
* There is also a macro setLED(led, state), which
* does exactly the same as this function.
*
* Example:
* setLED(1,4);
* // Switches LED1 ON
* // ... OR ...
* setLED(LED1,STATE_ON);
* // does the same.
*
*/
void TCA6507_set(uint8_t led, uint8_t state)
{
uint8_t i, mask;
if(led > 6) led = 0;
led = 1 << led;
mask = 1;
for(i = 0; i < 3; i++) {
select[i] &= ~led;
if(state & mask)
select[i] |= led;
mask <<= 1;
}
TCA6507_update();
}
/**
* If the LEDs are not needed for a while, the
* LED driver can be stopped with this function
* (TCA6507 set to shutdown mode).
*
*/
void TCA6507_shutdown(void)
{
outport.en_leddriv = false; // I/O 0.4: Low
MAX7311_update();
}
/**
* If the LED driver was stopped with the
* function TCA6507_shutdown() before, it can be
* (re)started again with this function.
*
*/
void TCA6507_restart(void)
{
outport.en_leddriv = true; // I/O 0.4: High
MAX7311_update();
}
/**
* Initializes the LED driver after power on by
* switching all LEDs off.
*
*/
void TCA6507_init(void)
{
select[0] = 0;
select[1] = 0;
select[2] = 0;
TCA6507_update();
}
/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):
// --------------------------------------------
// 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_SENSOR_LSM303DLHC_A_ADR, CTRL_REG1_A, CTRL_REG1_A_10HZ);
mSleep(10);
// Set HD mode:
I2CTWI_transmit2Bytes(I2C_SENSOR_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_SENSOR_LSM303DLHC_A_ADR, (OUT_X_L_A | 0x80));
I2CTWI_readBytes(I2C_SENSOR_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;
// 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;
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:
phi = asin(xa); // Calculate the and phi
the = 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_mag;
#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_SENSOR_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ_T_EN);
#else
// Set Data output rate to 15Hz:
I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ);
#endif
mSleep(10);
// Set Gain to +-1.3Gauss:
I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_M_ADR, CRB_REG_M, CRB_REG_M_13GAUSS);
mSleep(10);
// Set Continuous-conversion mode:
I2CTWI_transmit2Bytes(I2C_SENSOR_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_SENSOR_LSM303DLHC_M_ADR, OUT_X_H_M);
I2CTWI_readBytes(I2C_SENSOR_LSM303DLHC_M_ADR, readBuf, 6); // Read X-/Y-/Z-axis
// xb = x:
x_axism = (readBuf[OUT_X_H_M - OUT_X_H_M] << 8) + readBuf[OUT_X_L_M - OUT_X_H_M];
// yb = y:
y_axism = (readBuf[OUT_Y_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Y_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];
mSleep(10);
#ifdef GET_TEMP_M
// Read temperature raw value:
I2CTWI_transmitByte(I2C_SENSOR_LSM303DLHC_M_ADR, TEMP_OUT_H_M);
readBuf[0] = I2CTWI_readByte(I2C_SENSOR_LSM303DLHC_M_ADR);
I2CTWI_transmitByte(I2C_SENSOR_LSM303DLHC_M_ADR, TEMP_OUT_L_M);
readBuf[1] = I2CTWI_readByte(I2C_SENSOR_LSM303DLHC_M_ADR);
temperaturem = (readBuf[0] << 8) | readBuf[1];
temperaturem >>= 4;
if (temperaturem & 2048) // Calculate temperature
temperaturem = (temperaturem & 2047) - 2048;
else
temperaturem = temperaturem & 2047;
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
* RP6 Sensor Board's front edge with the two
* headlights) 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 RP6 Sensor Board's front edge with the two
* headlights) 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 LSM303DLHC.
*
*/
void LSM303DLHC_init(void)
{
LSM303DLHC_A_init(); // Accelerometer init
LSM303DLHC_M_init(); // Magnetometer init
}
/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):
/**
* Sets the LY330ALH mode of operation.
*
* Input: mode -> Mode [0..3]
*
*/
void LY330ALH_set_mode(uint8_t mode)
{
outport.pd_gyro = false; // I/O 0.6: Low
outport.st_gyro = false; // I/O 0.7: Low
if(mode & 1) outport.pd_gyro = true;
if(mode & 2) outport.st_gyro = true;
MAX7311_update();
}
/**
* Reads the ADC1 value from the RP6Base.
*
* Hints: - The RP6Base input ADC1 has to be
* connected to the USRBUS: Pin 2 on
* the RP6 mainboard!
* - The RP6Base_I2CSlave program has to
* be loaded into the RP6Base
* microcontroller!
*
*/
uint16_t getRP6Base_ADC1(void)
{
I2CTWI_readRegisters(I2C_RP6_BASE_ADR, I2C_REG_ADC_ADC1_L, registerBuf, 2);
return (registerBuf[0] + (registerBuf[1] << 8));
}
int16_t z_axisg;
/**
* Performs a LY330ALH gyroscope measurement and
* returns the angular rate (velocity).
* Positive values mean an angular rate while
* rotating LEFT and negative values while
* rotating RIGHT!
* The global integer variable z_axisg also
* contains the measurement result.
*
*/
int16_t LY330ALH_measure(void)
{
z_axisg = getRP6Base_ADC1(); // Read raw data
z_axisg -= ADCVAL_ZERORATE_LEVEL; // Calculate value
return (z_axisg);
}
/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):
/**
* Reads the TCA4311A Ready output.
* The function returns the state (true/false).
* The global variable readyBusbuff is also
* filled with the Ready output state.
*
* Hint: You don't have to use this function,
* if the task task_RP6SensorBoard() is
* called frequently out of the main
* loop, because readyBusbuff will be
* updated by this task!
*
*/
uint8_t readReadyBusbuff(void)
{
I2CTWI_transmitByte(I2C_SENSOR_PORTEXP_ADR, MAX7311_INPUT_PORT1);
readyBusbuff = I2CTWI_readByte(I2C_SENSOR_PORTEXP_ADR);
readyBusbuff &= RDY_BUSBUFF;
return readyBusbuff;
}
uint8_t errorBusbuff;
/**
* If the I2C bus for the I2C-MODULE (SCLOUT,
* SDAOUT) is not used or shall be isolated from
* the RP6 I2C bus, this can be done with this
* function.
*
* Hints: - The I2C-MODULE power is NOT switched
* off by this function!
* - If USE_PD5 is defined in the
* configuration header file, this
* function uses the RP6M256 portpin,
* that is connected to plug I/O: Pin 9
* instead of MAX7311 I/O 1.0!
* - If the global variable errorBusbuff
* returns true, the bus buffer could
* not be switched to shutdown mode!
*
*/
void TCA4311A_shutdown(void)
{
// Wait until the I2C bus is idle:
while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
#ifdef USE_PD5
IO_SENSOR_TCA4311A_EN_PORT &= ~IO_SENSOR_TCA4311A_EN_IN; // Low
#else
outport.en_busbuff = false; // I/O 1.0: Low
MAX7311_update();
#endif
errorBusbuff = readReadyBusbuff();
}
/**
* If the I2C bus for the I2C-MODULE (SCLOUT,
* SDAOUT) was isolated from the RP6 I2C bus with
* the function TCA4311A_shutdown() before, it
* can be connected again with this function.
*
* Hints: - The I2C-MODULE power is NOT switched
* on by this function!
* - If USE_PD5 is defined in the
* configuration header file, this
* function uses the RP6M256 portpin,
* that is connected to plug I/O: Pin 9
* instead of MAX7311 I/O 1.0!
* - If the global variable errorBusbuff
* returns true, the bus buffer could
* not be switched to normal operation!
*
*/
void TCA4311A_reconnect(void)
{
// Wait until the I2C bus is idle:
while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
#ifdef USE_PD5
IO_SENSOR_TCA4311A_EN_PORT |= IO_SENSOR_TCA4311A_EN_IN; // High
#else
outport.en_busbuff = true; // I/O 1.0: High
MAX7311_update();
#endif
errorBusbuff = !readReadyBusbuff();
}
/**
* Sets/clears the RP6Base signal PWR.
*
* Input: pwr -> 0 (false) = PWR off
* >0 (true) = PWR on
*
* Hints: - The RP6Base signal PWR has to be
* connected to the USRBUS: Pin 3 on
* the RP6 mainboard!
* - The RP6Base_I2CSlave program has to
* be loaded into the RP6Base
* microcontroller!
*
*/
void setRP6Base_PWR(uint8_t pwr)
{
if(pwr > 0)
I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_ON);
else
I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_OFF);
while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
}
/**
* With this function you can switch the I2C-
* MODULE (plug M1) power on or off.
*
* Input: pwr -> 0 (false) = I2C-MODULE power off
* >0 (true) = I2C-MODULE power on
*
* Hints: - If the I2C-MODULE plug is not used,
* you should always switch the
* I2C-MODULE power off (default)!
* - The TCA4311A is NOT switched to
* normal operation (default) or
* shutdown mode by this function!
* - The RP6Base signal PWR has to be
* connected to the USRBUS: Pin 3 on
* the RP6 mainboard!
* - The RP6Base_I2CSlave program has to
* be loaded into the RP6Base
* microcontroller!
*
*/
void setI2CModulePower(uint8_t pwr)
{
if(pwr > 0) {
setRP6Base_PWR(1); // RP6Base PWR on
outport.pwr_i2c = true; // I/O 0.0: High
}
else
outport.pwr_i2c = false; // I/O 0.0: Low
MAX7311_update();
}
/*****************************************************************************/
// Button:
/**
* Reads the MAX7311 I/O port 1.7.
* The function returns true, if button S1 is
* pressed (I/O 1.7: Low!) and false, if it is
* not pressed (I/O 1.7: High!).
* The global variable pressedButton is also
* filled with the I/O 1.7 output state.
*
* Hint: You don't have to use this function,
* if the task task_RP6SensorBoard() is
* called frequently out of the main
* loop, because pressedButton will be
* updated by this task!
*
*/
uint8_t readButton(void)
{
I2CTWI_transmitByte(I2C_SENSOR_PORTEXP_ADR, MAX7311_INPUT_PORT2);
pressedButton = I2CTWI_readByte(I2C_SENSOR_PORTEXP_ADR);
pressedButton &= IO2_I2C;
pressedButton = !pressedButton;
return pressedButton;
}
/*****************************************************************************/
// Status LED:
/**
* Sets the status LED (D2) of the RP6 Sensor
* Board.
*
* Example:
* setStatusLED(1);
* // This switches the status LED on!
*
*/
void setStatusLED(uint8_t on)
{
if(on > 0)
outport.io1_i2c = false; // I/O 1.6: Low
else
outport.io1_i2c = true; // I/O 1.6: High
MAX7311_update();
}
/*****************************************************************************/
/*****************************************************************************/
// RP6 Sensor Board initialisation and shutdown:
/**
* You MUST call this function at the beginning of a
* main program, that uses the RP6 Sensor Board.
*
*/
void sensor_board_init(void)
{
// I/O Port Expander:
MAX7311_init();
// RTC:
DS1339_init();
// Temperature Sensor:
LM75B_init();
// Servo Controller:
PCA9685_init(50); // Init PWM 50 Hz
// LED Driver:
TCA6507_init();
// 3D Magnetometer & Accelerometer:
LSM303DLHC_init();
}
/**
* If you don't use any function of the RP6
* Sensor Board, you can put the board into
* "SHUTDOWN MODE". In this mode the electric
* power consumption is very low to save energy.
*
*/
void sensor_board_shutdown(void)
{
// Servo Controller:
PCA9685_shutdown(); // Shutdown PCA9685
// Temperature Sensor:
LM75B_shutdown(); // Shutdown LM75B
// I/O Port Expander:
MAX7311_write(MAX7311_OUTPUT_SHUTDOWN);
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.0 TCA4311A reconnect problem fixed 10.04.2014 by Dirk
* - v. 0.9 Interrupt status functions added 09.04.2014 by Dirk
* - v. 0.8 (initial release) 17.03.2014 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6M256_Sensor_Board_01 ... SRC += $(RP6_LIB_PATH)/RP6common/RP6I2CmasterTWI.c SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256_Sensor_BoardLib.c ...
Datei: RP6M256_Sensor_Board_01.c
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 Sensor Board
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a first test for the RP6 Sensor 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 Sensor Board library":
// (This is the library for accessing the RP6 Sensor Board!)
#include "RP6M256_Sensor_BoardLib.h"
/*****************************************************************************/
// Variables:
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\nRP6 Sensor Board 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("RP6 Sensor Board", " Selftest 1");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
uint8_t onoff = 0;
uint16_t servopos = SERVO1_LT;
int16_t rate;
startStopwatch1();
startStopwatch2();
// IMPORTANT:
sensor_board_init(); // RP6 Sensor Board init!!!
setServoPower(1); // Servo power ON!
/*
// ----------------------------------------------
// Set RTC once (battery empty or not existing:
rtc_time.second = 0;
rtc_time.minute = 15;
rtc_time.hour = 20; // 20:15
rtc_date.weekday = R_SU;
rtc_date.day = 20;
rtc_date.month = 4;
rtc_date.year = 2014; // Su, 20.4.2014
DS1339_write();
// Remove this, if RTC is set and running!!!
// ----------------------------------------------
*/
while(true)
{
if(getStopwatch1() > 1000) // 1s
{
if (onoff) onoff = 0;
else onoff = 1;
// Interrupt status:
writeString_P_WIFI("\nInterrupt Status: ");
writeIntegerLength_WIFI(interrupt_RP6SBstatus.byte, BIN, 8);
writeString_P_WIFI("\n");
// Inport status:
writeString_P_WIFI("Inport Status: ");
writeIntegerLength_WIFI(inport.word, BIN, 16);
writeString_P_WIFI("\n");
// Temperature sensor test:
temperature = LM75B_measure(); // Measure
writeString_P_WIFI("Temperature: ");
writeDouble_WIFI(temperature, 7, 3);
writeString_P_WIFI("°\n");
// LED driver test:
if (onoff) {
setLED(HEADLIGHT_R, STATE_ON);
setLED(HEADLIGHT_L, STATE_OFF);
}
else {
setLED(HEADLIGHT_L, STATE_ON);
setLED(HEADLIGHT_R, STATE_OFF);
}
// Servo controller test (SERVO_1):
setServo(1, servopos);
servopos += 10;
if (servopos > SERVO1_RT) servopos = SERVO1_LT;
// RTC test:
DS1339_read();
writeString_P_WIFI("RTC: ");
writeIntegerLength_WIFI(rtc_time.hour, DEC, 2);
writeString_P_WIFI(":");
writeIntegerLength_WIFI(rtc_time.minute, DEC, 2);
writeString_P_WIFI(":");
writeIntegerLength_WIFI(rtc_time.second, DEC, 2);
writeString_P_WIFI(" ");
writeIntegerLength_WIFI(rtc_date.day, DEC, 2);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(rtc_date.month, DEC, 2);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(rtc_date.year, DEC, 4);
writeString_P_WIFI("\n");
// 3D Magnetometer & Accelerometer test:
// 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');
// 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_mag = (double) temperaturem / 8.0 + OFFSET_TEMP_M;
writeString_P_WIFI("Temperature [°]: ");
writeDouble_WIFI(temperature_mag, 7, 2);
writeChar_WIFI('\n');
#endif
// Yaw-Rate Gyroscope test:
rate = LY330ALH_measure();
writeString_P_WIFI("\n1D-Gyro SENSOR ->\n");
writeString_P_WIFI("Z-axis: ");
writeInteger_WIFI(rate, DEC);
writeString_P_WIFI("\n");
// RP6 Sensor Board shutdown (button & status LED test):
if (pressedButton) {
setStatusLED(1);
writeString_P_WIFI("\nPress button again for RP6 Sensor Board SHUTDOWN!\n");
mSleep(2000);
do {
if(getStopwatch2() > 200) { // 0.2s
if (onoff) onoff = 0;
else onoff = 1;
if (onoff) setStatusLED(1);
else setStatusLED(0);
setStopwatch2(0);
}
mSleep(1);
task_I2CTWI();
} while (!readButton());
writeString_P_WIFI("\n\n\n\n\nPlease wait for RP6 Sensor Board SHUTDOWN...\n");
sensor_board_shutdown();
mSleep(3000);
writeString_P_WIFI("\n\n\nThe RP6 Sensor Board now is in SHUTDOWN MODE!!!\n");
mSleep(1000);
writeString_P_WIFI("\nRESET the M256 microcontroller now...\n\n");
while(true) {};
}
setStopwatch1(0);
}
task_checkINTs(); // Read interrupt signals
task_RP6SensorBoard(); // RP6 Sensor Board task
task_I2CTWI();
}
return 0;
}
Erklärung
Projekte
RP6 Xtra Module
Die Dokumentation zu den RP6 Xtra Modulen befindet sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM". Auf der CD-ROM sind auch Software-Beispiele zu finden.
Die hier veröffentlichte Software zu den Xtra Modulen ist eine Eigenentwicklung in GCC:
- Arexx I2C GPS Empfänger JM3-GPS
- Arexx Gyro Modul JM3-GYRO
- Arexx 3D Accelerometer Modul JM3-3DA
I2C GPS Empfänger JM3-GPS
RP6M256 WiFi: I2C_GPS Library
Die nachfolgende RP6M256_I2C_GPSLib (GCC) dient zur Ansteuerung des I2C GPS Empfängers durch die RP6M256 WiFi.
Libary Header
Datei: RP6M256_I2C_GPSLib.h
/* ****************************************************************************
* _______________________
* \| RP6 ROBOT SYSTEM |/
* \_-_-_-_-_-_-_-_-_-_/ >>> RP6 M256 WIFI
* ----------------------------------------------------------------------------
* ----------------------------- [c]2014 - Dirk -------------------------------
* ****************************************************************************
* File: RP6M256_I2C_GPSLib.h
* Version: 1.0
* Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
* Author(s): Dirk
* ****************************************************************************
* Description:
* Header file for new RP6 I2C GPS Receiver library.
*
* ****************************************************************************
*/
#ifndef RP6M256_I2C_GPSLIB_H
#define RP6M256_I2C_GPSLIB_H
/*****************************************************************************/
// RP6 Sensor Board:
// If you want to use the RP6 I2C GPS Receiver on the RP6 Sensor Board, you
// have to define SENSOR_BOARD (default)!
// If SENSOR_BOARD is NOT defined, this library assumes that the RP6 I2C GPS
// Receiver is used stand-alone.
// ------------------
#define SENSOR_BOARD
// ------------------
/*****************************************************************************/
// Includes:
#include "RP6M256Lib.h"
#include "RP6I2CmasterTWI.h"
#ifdef SENSOR_BOARD
#include "RP6M256_Sensor_BoardLib.h"
#endif
/*****************************************************************************/
// RP6 I2C GPS Receiver:
#define I2C_GPS_RECEIVER_ADR 0xc8 // Default
//#define I2C_GPS_RECEIVER_ADR 0xca
//#define I2C_GPS_RECEIVER_ADR 0xcc
//#define I2C_GPS_RECEIVER_ADR 0xce
//#define I2C_GPS_RECEIVER_ADR 0xd0 // DO NOT USE on Sensor Board!
//#define I2C_GPS_RECEIVER_ADR 0xd2
//#define I2C_GPS_RECEIVER_ADR 0xd4
//#define I2C_GPS_RECEIVER_ADR 0xd6
//#define I2C_GPS_RECEIVER_ADR 0xd8
//#define I2C_GPS_RECEIVER_ADR 0xda
//#define I2C_GPS_RECEIVER_ADR 0xdc
//#define I2C_GPS_RECEIVER_ADR 0xde
// Read registers:
#define WHO_AM_I 0x20 // I2C address [0xc8..0xde]
#define LAT_DEG 0x21 // Degrees
#define LAT_MIN 0x22 // Minutes
#define LAT_MIN_D_L 0x23 // Minutes decimal part low
#define LAT_MIN_D_H 0x24 // Minutes decimal part high
#define LON_DEG 0x25 // Degrees
#define LON_MIN 0x26 // Minutes
#define LON_MIN_D_L 0x27 // Minutes decimal part low
#define LON_MIN_D_H 0x28 // Minutes decimal part high
#define STATUS 0x29 // Status flags
#define FIX_IND 0x2a // FIX_IND
#define HDOP 0x2b // HDOP
#define HDOP_D 0x2c // HDOP_D
#define SAT_N 0x2d // Number of satellites
#define COURSE_T_L 0x2e // True heading low byte
#define COURSE_T_H 0x2f // True heading high byte
#define COURSE_T_D 0x30 // Decimal part of course
#define SPEED_L 0x31 // Speed low byte
#define SPEED_H 0x32 // Speed high byte
#define SPEED_D 0x33 // Speed decimal part
#define ALT_L 0x34 // Altitude low byte
#define ALT_H 0x35 // Altitude high byte
#define ALT_DIF_L 0x36 // Differ. WGS-84 MSL low byte
#define ALT_DIF_H 0x37 // Differ. WGS-84 MSL high byte
#define UTC_H 0x38 // UTC hour
#define UTC_MIN 0x39 // UTC minute
#define UTC_S 0x3a // UTC second
#define DAY 0x3b // Day
#define YEAR_L 0x3c // Year low byte
#define YEAR_H 0x3d // Year high byte
#define MONTH 0x3e // Month
#define PDOP 0x3f // PDOP
#define PDOP_D 0x40 // PDOP_D
#define VDOP 0x41 // VDOP
#define VDOP_D 0x42 // VDOP_D
#define SAT_1 0x43 // SAT # 1
#define SAT_2 0x44 // SAT # 2
#define SAT_3 0x45 // SAT # 3
#define SAT_4 0x46 // SAT # 4
#define SAT_5 0x47 // SAT # 5
#define SAT_6 0x48 // SAT # 6
#define SAT_7 0x49 // SAT # 7
#define SAT_8 0x4a // SAT # 8
#define SAT_9 0x4b // SAT # 9
#define SAT_A 0x4c // SAT # 10
#define SAT_B 0x4d // SAT # 11
#define SAT_C 0x4e // SAT # 12
#define SW_VERSION 0x4f // SW_Version
#define HEARTBEAT 0x50 // Heartbeat
// Status and Fix_Ind register bitmasks:
#define STATUS_NS 0b00000001
#define STATUS_EW 0b00000010
#define STATUS_SPU 0b00000100
#define STATUS_AUTON 0b00001000
#define FIX_IND_UI_FIX 0b10000000
// Write registers:
#define INIT_CFG 0x90 // INIT CFG
#define SET_FIX_TIME 0x91 // Pos_Fix_Time
#define STBY 0x92 // STBY
#define FAC_RES 0x93 // Factory Reset
#define EN_JAM 0x94 // Jamming Remover
#define TEST_MODE 0x96 // Test Mode
// Write register values (commands):
#define INIT_CFG_HOT_START 1 // Hot Start
#define INIT_CFG_WARM_START 2 // Warm Start init data loaded
#define INIT_CFG_WARM_START_EPH_CLR 3 // Warm Start Ephemeris cleared
#define INIT_CFG_COLD_START 4 // Cold Start
#define SET_FIX_TIME_1S 1 // Pos Fix Time 1 second
#define SET_FIX_TIME_2S 2 // Pos Fix Time 2 seconds
#define STBY_ENTER 1 // Standby (cleared after write)
#define FAC_RES_ENTER 1 // Factory Reset
#define EN_JAM_DISABLE 0 // Jamming Remover disable
#define EN_JAM_ENABLE 1 // Jamming Remover enable
#define TEST_MODE_INACTIVE 0 // Test Mode inactive
#define TEST_MODE_ACTIVE 0x15 // Test Mode active
// Data structure (starts at LAT_DEG):
#define DATA_STRUCT LAT_DEG // Start
// All data from LAT_DEG to HEARTBEAT (48 bytes):
#define DATA_LENGTH (HEARTBEAT - LAT_DEG + 1)
// Basic data from LAT_DEG to SAT_N (13 bytes):
#define DATA_BASIC (SAT_N - LAT_DEG + 1)
typedef union {
uint8_t byte;
struct {
unsigned ns :1; // 1: North 0: South
unsigned ew :1; // 1: East 0: West
unsigned spu :1; // 1: km/h 0: kn
unsigned auton :1; // 1: Data not valid 0: Autonomous
unsigned fix0 :1;
unsigned fix1 :1;
unsigned stat0 :1; // Invalid
unsigned stat1 :1; // GPS Com Error
};
struct {
unsigned ns :1; // 1: North 0: South
unsigned ew :1; // 1: East 0: West
unsigned spu :1; // 1: km/h 0: kn
unsigned auton :1; // 1: Data not valid 0: Autonomous
unsigned fix :2;
unsigned stat :2; // 00 -> Valid
// 01 -> Invalid
// 10 -> GPS Com Error
// 11 -> n. def.
};
} status_t;
typedef union {
uint8_t byte;
struct { // Fix indicator:
unsigned fix_0 :1; // 1: GPS fix 2: DGPS fix
unsigned fix_1 :1; // 3: PPS fix 4: RTK
unsigned fix_2 :1; // 5: Float RTK 6: Dead reck.
unsigned fix_3 :1; // 7: Manual inp. mode 8: Simul. mode
unsigned unused :3;
unsigned ui_fix :1; // HW signal from UC530
};
struct {
unsigned fix :4; // See above "Fix indicator"!
unsigned unused :3;
unsigned ui_fix :1; // HW signal from UC530
};
} fix_ind_t;
typedef union {
struct {
uint8_t latDeg;
uint8_t latMin;
uint16_t latMinDec;
uint8_t lonDeg;
uint8_t lonMin;
uint16_t lonMinDec;
status_t status;
fix_ind_t fixInd;
uint8_t hdop;
uint8_t hdopDec;
uint8_t satCount;
uint16_t course;
uint8_t courseDec;
uint16_t speed;
uint8_t speedDec;
uint16_t altitude;
uint16_t altDif;
uint8_t utcHour;
uint8_t utcMinute;
uint8_t utcSec;
uint8_t day;
uint16_t year;
uint8_t month;
uint8_t pdop;
uint8_t pdopDec;
uint8_t vdop;
uint8_t vdopDec;
uint8_t sat[12];
uint8_t swVersion;
uint8_t heartbeat;
};
} gpsdata_t;
extern gpsdata_t gpsdata;
void I2C_GPS_reset(void);
void I2C_GPS_write_cmd(uint8_t, uint8_t);
uint8_t I2C_GPS_read_reg(uint8_t);
void I2C_GPS_read(uint8_t);
void I2C_GPS_init(void);
void I2C_GPS_new_addr(uint8_t);
/*****************************************************************************/
#endif
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
*
* ---> changes are documented in the file "RP6M256_I2C_GPSLib.c"
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Libary Source
Datei: RP6M256_I2C_GPSLib.c
/* ****************************************************************************
* _______________________
* \| RP6 ROBOT SYSTEM |/
* \_-_-_-_-_-_-_-_-_-_/ >>> RP6 M256 WIFI
* ----------------------------------------------------------------------------
* ----------------------------- [c]2014 - Dirk -------------------------------
* ****************************************************************************
* File: RP6M256_I2C_GPSLib.c
* Version: 1.0
* Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
* and a RP6 I2C GPS Receiver (CONRAD 1082385)
* optionally on a RP6 Sensor Board (CONRAD 1082384)
* Author(s): Dirk
* ****************************************************************************
* Description:
*
* This is our new Library that contains basic routines and functions for
* accessing the RP6 I2C GPS Receiver JM3-GPS (CONRAD 1082385).
* The library assumes the I2C GPS Receiver software (firmware) v1.2.
* If the I2C GPS Receiver is located on the RP6 Sensor Board (CONRAD 1082384)
* and connected to the I2C-MODULE plug M1, the RP6M256_Sensor_BoardLib has to
* be included!
*
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6M256_I2C_GPSLib.h"
/*****************************************************************************/
// Variables:
uint8_t gpsdataBuf[DATA_LENGTH + 1];
/*****************************************************************************/
/**
* Software reset for the I2C GPS Receiver.
*
*/
void I2C_GPS_reset(void)
{
I2CTWI_transmit2Bytes(I2C_GPS_RECEIVER_ADR, 0, 0x06);
}
/**
* Writes a command cmd to a register reg of the
* I2C GPS Receiver.
*
* Input: reg -> Write register [0x90..0x96]
* cmd -> Command
*
*/
void I2C_GPS_write_cmd(uint8_t reg, uint8_t cmd)
{
if((reg < INIT_CFG) || (reg > TEST_MODE)) return;
I2CTWI_transmit2Bytes(I2C_GPS_RECEIVER_ADR, reg, cmd);
}
/**
* Reads a single register reg from the I2C GPS
* Receiver.
*
* Input: reg -> Read register [0x20..0x50]
*
*/
uint8_t I2C_GPS_read_reg(uint8_t reg)
{
if((reg < WHO_AM_I) || (reg > HEARTBEAT)) reg = WHO_AM_I;
I2CTWI_transmitByte(I2C_GPS_RECEIVER_ADR, reg);
return (I2CTWI_readByte(I2C_GPS_RECEIVER_ADR));
}
gpsdata_t gpsdata;
/**
* Reads the GPS data from the I2C GPS Receiver.
*
* Input: length -> Number of data bytes [1..48]
*
* Hint: If length = DATA_BASIC, only basic GPS
* data are read. If length = DATA_LENGTH,
* ALL GPS data are read.
*
*/
void I2C_GPS_read(uint8_t length)
{
uint8_t i;
if(length > DATA_LENGTH) length = DATA_LENGTH;
I2CTWI_readRegisters(I2C_GPS_RECEIVER_ADR, DATA_STRUCT, gpsdataBuf, length);
for(i = length; i < DATA_LENGTH; i++) {
gpsdataBuf[i] = 0;
}
gpsdata.latDeg = gpsdataBuf[0];
gpsdata.latMin = gpsdataBuf[1];
gpsdata.latMinDec = gpsdataBuf[2] + (gpsdataBuf[3] << 8);
gpsdata.lonDeg = gpsdataBuf[4];
gpsdata.lonMin = gpsdataBuf[5];
gpsdata.lonMinDec = gpsdataBuf[6] + (gpsdataBuf[7] << 8);
gpsdata.status.byte = gpsdataBuf[8];
gpsdata.fixInd.byte = gpsdataBuf[9];
gpsdata.hdop = gpsdataBuf[10];
gpsdata.hdopDec = gpsdataBuf[11];
gpsdata.satCount = gpsdataBuf[12];
if(length == DATA_BASIC) return;
gpsdata.course = gpsdataBuf[13] + (gpsdataBuf[14] << 8);
gpsdata.courseDec = gpsdataBuf[15];
gpsdata.speed = gpsdataBuf[16] + (gpsdataBuf[17] << 8);
gpsdata.speedDec = gpsdataBuf[18];
gpsdata.altitude = gpsdataBuf[19] + (gpsdataBuf[20] << 8);
gpsdata.altDif = gpsdataBuf[21] + (gpsdataBuf[22] << 8);
gpsdata.utcHour = gpsdataBuf[23];
gpsdata.utcMinute = gpsdataBuf[24];
gpsdata.utcSec = gpsdataBuf[25];
gpsdata.day = gpsdataBuf[26];
gpsdata.year = gpsdataBuf[27] + (gpsdataBuf[28] * 100);
gpsdata.month = gpsdataBuf[29];
gpsdata.pdop = gpsdataBuf[30];
gpsdata.pdopDec = gpsdataBuf[31];
gpsdata.vdop = gpsdataBuf[32];
gpsdata.vdopDec = gpsdataBuf[33];
for(i = 34; i < 46; i++) {
gpsdata.sat[i - 34] = gpsdataBuf[i];
}
gpsdata.swVersion = gpsdataBuf[46];
gpsdata.heartbeat = gpsdataBuf[47];
}
/**
* You MUST call this function at the beginning of a
* main program, that uses the RP6 I2C GPS Receiver.
* If SENSOR_BOARD is defined, the I2C-MODULE power
* will be switched on with the function
* setI2CModulePower() defined in the
* RP6M256_Sensor_BoardLib, which will be included
* then.
*
*/
void I2C_GPS_init(void)
{
#ifdef SENSOR_BOARD
// I2C-MODULE (plug M1) power on:
setI2CModulePower(1);
#endif
// Other init procedures here!
}
/**
* Writes a new I2C address to the I2C GPS Receiver.
*
* Input: addr -> New I2C address [0xc8..0xde]
*
* Hints: - Default I2C address is 0xc8.
* - You may choose a new I2C address in
* the range 0xc8..0xde.
* - After writing a new address you have
* to switch the I2C GPS Receiver off
* and on again.
* - Definition I2C_GPS_RECEIVER_ADR must
* be adapted to the new I2C address!
*
*/
void I2C_GPS_new_addr(uint8_t addr)
{
if((addr < 0xc8) || (addr > 0xde)) return;
addr &= 0xfe; // Clear R/W bit
I2CTWI_transmit3Bytes(I2C_GPS_RECEIVER_ADR, 0, 0x04, addr);
}
/*****************************************************************************/
/*****************************************************************************/
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.0 (initial release) 21.04.2014 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6M256_Sensor_Board_02 ... SRC += $(RP6_LIB_PATH)/RP6common/RP6I2CmasterTWI.c SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256_Sensor_BoardLib.c SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256_I2C_GPSLib.c ...
Datei: RP6M256_Sensor_Board_02.c
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
* ****************************************************************************
* Example: RP6M256 Sensor Board
* Author(s): Dirk
* ****************************************************************************
* Description:
* In this example we show a second test for the RP6 Sensor 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 I2C GPS Receiver library":
// (This is the library for accessing the RP6 I2C GPS Receiver!)
#include "RP6M256_I2C_GPSLib.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]);
}
/**
* 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\nRP6 Sensor Board 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("RP6 Sensor Board", " Selftest 2");
mSleep(2500);
clearLCD();
setLEDs(0b0000);
// ---------------------------------------
uint8_t onoff = 0;
uint8_t i;
startStopwatch1();
startStopwatch2();
// IMPORTANT:
sensor_board_init(); // RP6 Sensor Board init!!!
I2C_GPS_init(); // RP6 I2C GPS Receiver init
while(true)
{
if(getStopwatch1() > 1000) // 1s
{
if (onoff) onoff = 0;
else onoff = 1;
// Show GPS data:
I2C_GPS_read(DATA_LENGTH); // Read data
writeString_P_WIFI("\nLAT_DEG: ");
writeIntegerLength_WIFI(gpsdata.latDeg, DEC, 2);
writeString_P_WIFI("° ");
writeIntegerLength_WIFI(gpsdata.latMin, DEC, 2);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.latMinDec, DEC, 4);
writeString_P_WIFI("' ");
if(gpsdata.status.ns)
writeString_P_WIFI("N");
else
writeString_P_WIFI("S");
writeString_P_WIFI("\nLON_DEG: ");
writeIntegerLength_WIFI(gpsdata.lonDeg, DEC, 3);
writeString_P_WIFI("° ");
writeIntegerLength_WIFI(gpsdata.lonMin, DEC, 2);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.lonMinDec, DEC, 4);
writeString_P_WIFI("' ");
if(gpsdata.status.ew)
writeString_P_WIFI("E");
else
writeString_P_WIFI("W");
writeString_P_WIFI("\nSTATUS: ");
writeIntegerLength_WIFI(gpsdata.status.byte, BIN, 8);
writeString_P_WIFI("\nFIX_IND: ");
writeIntegerLength_WIFI(gpsdata.fixInd.byte, BIN, 8);
writeString_P_WIFI("\nHDOP: ");
writeInteger_WIFI(gpsdata.hdop, DEC);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.hdopDec, DEC, 2);
writeString_P_WIFI("\nSAT_N: ");
writeInteger_WIFI(gpsdata.satCount, DEC);
writeString_P_WIFI("\nCOURSE_T: ");
writeInteger_WIFI(gpsdata.course, DEC);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.courseDec, DEC, 2);
writeString_P_WIFI("°");
writeString_P_WIFI("\nSPEED: ");
writeInteger_WIFI(gpsdata.speed, DEC);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.speedDec, DEC, 2);
if(gpsdata.status.spu)
writeString_P_WIFI("km/h");
else
writeString_P_WIFI("kn");
writeString_P_WIFI("\nALT: ");
writeInteger_WIFI(gpsdata.altitude, DEC);
writeString_P_WIFI("m\nALT_DIF: ");
writeInteger_WIFI(gpsdata.altDif, DEC);
writeString_P_WIFI("m\nUTC: ");
writeIntegerLength_WIFI(gpsdata.utcHour, DEC, 2);
writeString_P_WIFI(":");
writeIntegerLength_WIFI(gpsdata.utcMinute, DEC, 2);
writeString_P_WIFI(":");
writeIntegerLength_WIFI(gpsdata.utcSec, DEC, 2);
writeString_P_WIFI("\nDATE: ");
writeIntegerLength_WIFI(gpsdata.day, DEC, 2);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.month, DEC, 2);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.year, DEC, 4);
writeString_P_WIFI("\nPDOP: ");
writeInteger_WIFI(gpsdata.pdop, DEC);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.pdopDec, DEC, 2);
writeString_P_WIFI("\nVDOP: ");
writeInteger_WIFI(gpsdata.vdop, DEC);
writeString_P_WIFI(".");
writeIntegerLength_WIFI(gpsdata.vdopDec, DEC, 2);
writeString_P_WIFI("\n");
for(i = 0; i < gpsdata.satCount; i++) {
if(i == 8) writeString_P_WIFI("\n");
writeString_P_WIFI(" SAT_");
writeIntegerLength_WIFI(i + 1, HEX, 1);
writeString_P_WIFI(": ");
writeInteger_WIFI(gpsdata.sat[i], DEC);
}
writeString_P_WIFI("\nSW_VERSION: ");
writeInteger_WIFI(gpsdata.swVersion, DEC);
writeString_P_WIFI("\nHEARTBEAT: ");
writeInteger_WIFI(gpsdata.heartbeat, DEC);
writeString_P_WIFI("\n");
// RP6 Sensor Board shutdown (button & status LED test):
if (pressedButton) {
setStatusLED(1);
writeString_P_WIFI("\nPress button again for RP6 Sensor Board SHUTDOWN!\n");
mSleep(2000);
do {
if(getStopwatch2() > 200) { // 0.2s
if (onoff) onoff = 0;
else onoff = 1;
if (onoff) setStatusLED(1);
else setStatusLED(0);
setStopwatch2(0);
}
mSleep(1);
task_I2CTWI();
} while (!readButton());
writeString_P_WIFI("\n\n\n\n\nPlease wait for RP6 Sensor Board SHUTDOWN...\n");
sensor_board_shutdown();
mSleep(3000);
writeString_P_WIFI("\n\n\nThe RP6 Sensor Board now is in SHUTDOWN MODE!!!\n");
mSleep(1000);
writeString_P_WIFI("\nRESET the M256 microcontroller now...\n\n");
while(true) {};
}
setStopwatch1(0);
}
task_RP6SensorBoard(); // RP6 Sensor Board task
task_I2CTWI();
}
return 0;
}
Erklärung
Gyro Modul JM3-GYRO
Analoge Auswertung
Am Pin OUT des Gyro Moduls (siehe Anschluss-Schema!) liegt die Ausgangsspannung des Sensors (0..5V) an. Sie kann mit einem ADC-Eingang eines Microcontrollers gemessen werden. Syntax mit den Libraries der RP6Base, M32 oder M256 WiFi (ADC Kanal 0):
uint16_t result = readADC(0);
In der C-Control-Pro Umgebung der M128 sieht das z.B. in CompactC so aus (ADC Kanal 0):
word result; ADC_Set(ADC_VREF_VCC, 0); ... result = ADC_Read();
Das Ergebnis result kann noch zur Weiterverarbeitung angepasst werden:
#define ADCVAL_ZERORATE_LEVEL 511 // Zero rate Level ... int16_t z_axisg = result - ADCVAL_ZERORATE_LEVEL;
Dadurch zeigt z_axisg Null an, wenn keine Rotation anliegt und wird positiv, wenn nach LINKS gedreht wird und negativ bei RECHTS-Drehung. Die Definition ADCVAL_ZERORATE_LEVEL muss natürlich an den eigenen Sensor-Nullwert angepasst werden.
Aus dem Absolutwert von z_axisg kann man die Drehrate (Drehgeschwindigkeit) auch bestimmen:
In den Technischen Daten ist die Empfindlichkeit des Sensors mit 10mV/dps (0,01V pro Grad und Sekunde) angegeben. Damit errechnet sich die Drehrate als:
Drehrate [dps] = abs(z_axisg) * (5 / 1024 / 0.01) = abs(z_axisg) * 0.48828125
Optionale Ansteuerung
Im Anschluss-Schema ist erkennbar, dass es noch 2 Steuerleitungen SLEEP/PD und ST gibt. Mit diesen Anschlüssen kann die Betriebsart des Gyro Moduls festgelegt werden, siehe nachfolgende Tabelle:
| Betriebsart | ST | SLEEP/PD | Stromverbrauch |
| Normal | 0 | 0 | 4,2mA |
| Sleep | 0 | 1 | 2,2mA |
| Self-test | 1 | 0 | 4,2mA |
| Power-down | 1 | 1 | 5µA |
Will man zwischen den Betriebsarten per Software umschalten, muss man die beiden Steuerleitungen mit I/O Portpins des Microcontrollers verbinden, die als Ausgänge konfiguriert sind. Braucht man diese Funktion nicht, reicht es, ST und SLEEP/PD mit GND zu verbinden (Betriebsart "Normal").
3D Accelerometer Modul JM3-3DA
Das 3D Accelerometer Modul kann über den I2C- oder den SPI-Bus angesteuert werden. Wird das Modul auf der RP6 CONTROL M32 Platine betrieben (es sitzt also auf dem Sockel IC5 für das 2. SPI-EEPROM), dann erfolgt die Ansteuerung über den SPI-Bus. Die SPI-Grundfunktionen befinden sich in der RP6ControlLib, die daher eingebunden werden muss.
RP6 CONTROL M32: ACCSENS Library
Die nachfolgende RP6ControlACCSENSLib (GCC) dient zur Ansteuerung des 3D Accelerometer Moduls durch die RP6 CONTROL M32.
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 eine Definition:
- EXP_STACK_1 -> Die RP6 CONTROL M32 sitzt auf dem vorderen Platinenstapel 1 des RP6
Im nachfolgenden Listing ist die Definition EXP_STACK_1 NICHT aktiv (d.h. die M32 sitzt auf dem hinteren Platinenstapel 2!). Dies muss an die eigene Hardware angepaßt werden. Wenn die RP6 CONTROL M32 auf dem vorderen Platinenstapel 1 des RP6 sitzt, muss die Zeile mit "EXP_STACK_1" "aktiviert" werden. Dazu entfernt man "//" am Zeilenanfang:
#define EXP_STACK_1
Bevor man den Beschleunigungssensor nutzen kann, muss man ihn kalibrieren:
Accelerometer:
- 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 °.
Nach dem Eintragen der Kalibrierungswerte muss das Programm neu kompiliert werden!
Datei: RP6ControlACCSENS.h
/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL * ---------------------------------------------------------------------------- * ----------------------------- [c]2014 - Dirk ------------------------------- * **************************************************************************** * File: RP6ControlACCSENS.h * Version: 1.0 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz * Author(s): Dirk * **************************************************************************** * Description: * Configuration header file for new ACCSENS library. * * **************************************************************************** * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE! * **************************************************************************** */ #ifndef RP6CONTROLACCSENS_H #define RP6CONTROLACCSENS_H /*****************************************************************************/ // Sensor position on the RP6 robot: // (If EXP_STACK_1 is defined, the RP6 CONTROL M32 is located on the front // stack for addon boards on the RP6 robot. If EXP_STACK_1 is NOT defined // (default), the RP6 CONTROL M32 is mounted on the rear stack.) // ------------------- //#define EXP_STACK_1 // ------------------- /*****************************************************************************/ // The following sensor is used: // - 3D Accelerometer Modul (CONRAD 1082387-62) with LIS302DLH 3-axis // accelerometer /*****************************************************************************/ // LIS302DLH accelerometer calibration data: #define MAX_X_A 32767 // Max. X-axis value #define MIN_X_A -32768 // Min. X-axis value #define MAX_Y_A 32767 // Max. Y-axis value #define MIN_Y_A -32768 // Min. Y-axis value #define MAX_Z_A 32767 // Max. Z-axis value #define MIN_Z_A -32768 // Min. Z-axis value #define OFFSET_PITCH_A 0.0 // Offset Pitch [°] #define OFFSET_ROLL_A 0.0 // Offset Roll [°] /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6ControlACCSENSLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Header
Datei: RP6ControlACCSENSLib.h
/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL * ---------------------------------------------------------------------------- * ----------------------------- [c]2014 - Dirk ------------------------------- * **************************************************************************** * File: RP6ControlACCSENSLib.h * Version: 1.0 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz * Author(s): Dirk * **************************************************************************** * Description: * This is the RP6ControlACCSENSLib header file. * You have to include this file, if you want to use the library * RP6ControlACCSENSLib.c in your own projects. * * **************************************************************************** * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE! * **************************************************************************** */ #ifndef RP6CONTROLACCSENSLIB_H #define RP6CONTROLACCSENSLIB_H /*****************************************************************************/ // Includes: #include "RP6ControlLib.h" // The RP6 Control Library (v. 1.1 or higher). #include "RP6ControlACCSENS.h" #include <math.h> /*****************************************************************************/ #define ACC_CS MEM_CS2 #define DEVICE_IDENTIFIER 0x32 // 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 HP_FILTER_RESET 0x25 #define REFERENCE 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 INT1_CFG 0x30 #define INT1_SOURCE 0x31 #define INT1_THS 0x32 #define INT1_DURATION 0x33 #define INT2_CFG 0x34 #define INT2_SOURCE 0x35 #define INT2_THS 0x36 #define INT2_DURATION 0x37 // Control Register bitmasks: #define CTRL_REG1_POWERDOWN 0b00000000 #define CTRL_REG1_3D_DEFAULT 0b00000111 #define CTRL_REG1_3D_LOPWR_10HZ 0b11000111 #define CTRL_REG2_DEFAULT 0b00000000 #define CTRL_REG3_DEFAULT 0b00000000 #define CTRL_REG4_DEFAULT 0b00000000 #define CTRL_REG4_BDU_2G 0b10000000 #define CTRL_REG4_BDU_2G_STSIGN 0b10001000 #define CTRL_REG4_BDU_BLE_2G 0b11000000 #define CTRL_REG4_BDU_BLE_2G_STSIGN 0b11001000 #define CTRL_REG5_DEFAULT 0b00000000 // Read/write address bitmasks: #define READ_MASK 0b10000000 #define READ_AUTOINC_MASK 0b11000000 #define WRITE_MASK 0b01111111 extern int16_t x_axisa, y_axisa, z_axisa; extern double pitch, roll; extern double xa, ya, za; extern double the, phi; uint8_t SPI_ACCSENS_readByte(uint8_t regAddr); void SPI_ACCSENS_readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t length); void SPI_ACCSENS_writeByte(uint8_t regAddr, uint8_t data); uint8_t SPI_ACCSENS_getStatus(void); void SPI_ACCSENS_init(void); void SPI_ACCSENS_shutdown(void); void SPI_ACCSENS_read(void); void SPI_ACCSENS_normalize(void); void SPI_ACCSENS_position(void); /*****************************************************************************/ #endif /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * * ---> changes are documented in the file "RP6ControlACCSENSLib.c" * * **************************************************************************** */ /*****************************************************************************/ // EOF
Library Source
Datei: RP6ControlACCSENSLib.c
/* ****************************************************************************
* _______________________
* \| RP6 ROBOT SYSTEM |/
* \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL
* ----------------------------------------------------------------------------
* ----------------------------- [c]2014 - Dirk -------------------------------
* ****************************************************************************
* File: RP6ControlACCSENSLib.c
* Version: 1.0
* Target: RP6 CONTROL - ATMEGA32 @16.00MHz
* with optional LC-Display 16x2 chars (CONRAD 190911-62)
* and a 3D Accelerometer Modul (CONRAD 1082387-62) (IC5)
* Author(s): Dirk
* ****************************************************************************
* Description:
* This is my RP6 Control ACCSENS Library. This library may be used to access
* the basic functions of the 3D Accelerometer Modul. This RP6 Xtra module may
* be placed on the IC5 socket on the RP6 CONTROL M32 instead of a second
* EEPROM.
*
* ****************************************************************************
* THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6ControlACCSENSLib.h"
/*****************************************************************************/
// SPI functions:
/**
* Reads a single register from the 3D Accelerometer
* Modul (ACCSENS).
*
*/
uint8_t SPI_ACCSENS_readByte(uint8_t regAddr)
{
uint8_t data;
regAddr |= READ_MASK; // Read byte
PORTB &= ~ACC_CS;
writeSPI(regAddr);
data = readSPI();
PORTB |= ACC_CS;
return data;
}
/**
* Reads "length" bytes into the buffer "buffer"
* from startAddr on.
*
*/
void SPI_ACCSENS_readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t length)
{
startAddr |= READ_AUTOINC_MASK; // Read byte, auto increment
PORTB &= ~ACC_CS;
writeSPI(startAddr);
readBufferSPI(&buffer[0], length);
PORTB |= ACC_CS;
}
/**
* Write a single data byte to the specified sensor
* address.
*
*/
void SPI_ACCSENS_writeByte(uint8_t regAddr, uint8_t data)
{
PORTB &= ~ACC_CS;
writeSPI(regAddr);
writeSPI(data);
PORTB |= ACC_CS;
}
/**
* Returns sensor status register - for checking if
* sensor is busy.
*
*/
uint8_t SPI_ACCSENS_getStatus(void)
{
uint8_t status;
PORTB &= ~ACC_CS;
writeSPI(STATUS_REG);
status = readSPI();
PORTB |= ACC_CS;
return status;
}
/*****************************************************************************/
// 3D accelerometer functions:
int16_t x_axisa, y_axisa, z_axisa;
double pitch, roll;
double xa, ya, za;
double the, phi;
/**
* Initializes the sensor. You must call this
* function at the beginning of a program, that
* uses the 3D Accelerometer Modul.
*
*/
void SPI_ACCSENS_init(void)
{
// All axes enable, low power, 10Hz output data rate:
SPI_ACCSENS_writeByte(CTRL_REG1, CTRL_REG1_3D_LOPWR_10HZ);
// No update while reading, +- 2g, self-test minus:
SPI_ACCSENS_writeByte(CTRL_REG4, CTRL_REG4_BDU_2G_STSIGN);
}
/**
* Sets the LIS302DLH power-down mode. If you
* do not need the sensor, you can save energy
* by using the sensor power-down mode.
*
*/
void SPI_ACCSENS_shutdown(void)
{
// LIS302DLH in power-down mode:
SPI_ACCSENS_writeByte(CTRL_REG1, CTRL_REG1_POWERDOWN);
}
/**
* This function reads the X-axis, Y-axis and Z-axis
* values from the LIS302DLH accelerometer and stores
* them in the global variables x_axisa, y_axisa and
* z_axisa.
*
*/
void SPI_ACCSENS_read(void)
{
#ifndef EXP_STACK_1
// Default sensor position (M32 located on the RP6 rear stack):
// xb = -y:
x_axisa = SPI_ACCSENS_readByte(OUT_Y_L) | (SPI_ACCSENS_readByte(OUT_Y_H) << 8);
x_axisa *= -1;
// yb = -x:
y_axisa = SPI_ACCSENS_readByte(OUT_X_L) | (SPI_ACCSENS_readByte(OUT_X_H) << 8);
y_axisa *= -1;
// zb = z:
z_axisa = SPI_ACCSENS_readByte(OUT_Z_L) | (SPI_ACCSENS_readByte(OUT_Z_H) << 8);
#else
// Other sensor position (M32 located on the RP6 front stack):
// xb = y:
x_axisa = SPI_ACCSENS_readByte(OUT_Y_L) | (SPI_ACCSENS_readByte(OUT_Y_H) << 8);
// yb = x:
y_axisa = SPI_ACCSENS_readByte(OUT_X_L) | (SPI_ACCSENS_readByte(OUT_X_H) << 8);
// zb = z:
z_axisa = SPI_ACCSENS_readByte(OUT_Z_L) | (SPI_ACCSENS_readByte(OUT_Z_H) << 8);
#endif
}
/**
* This function limits and normalizes the x-axisa,
* y-axisa and z-axisa values read from the LIS302DLH
* accelerometer with the function SPI_ACCSENS_read() 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 SPI_ACCSENS_normalize(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 / 10000.0;
ya = (double) y_axisa / 10000.0;
za = (double) z_axisa / 10000.0;
// Calculate Pitch and Roll:
phi = asin(xa); // Calculate the and phi
the = asin(ya / cos(the)); // from the raw values
}
/**
* This function calculates the position in space by
* using the the and phi values (see function
* SPI_ACCSENS_normalize()!). It stores the position
* in the global double variables pitch and roll.
*
*/
void SPI_ACCSENS_position(void)
{
pitch = the * 180 / M_PI + OFFSET_PITCH_A;
roll = phi * 180 / M_PI + OFFSET_ROLL_A;
}
/******************************************************************************
* Additional info
* ****************************************************************************
* Changelog:
* - v. 1.0 (initial release) 04.04.2014 by Dirk
*
* ****************************************************************************
*/
/*****************************************************************************/
// EOF
Erklärung
Demo
makefile:
... TARGET = RP6Control_01_ACCSENS ... SRC += $(RP6_LIB_PATH)/RP6control/RP6ControlLib.c SRC += $(RP6_LIB_PATH)/RP6common/RP6uart.c SRC += $(RP6_LIB_PATH)/RP6control/RP6ControlACCSENSLib.c ...
Datei: RP6Control_01_ACCSENS.c
// Uncommented Version of RP6Control_01_ACCSENS.c
// written by Dirk
// ------------------------------------------------------------------------------------------
#include "RP6ControlACCSENSLib.h"
void writeDouble(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString(&buffer[0]);
}
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
int main(void)
{
initRP6Control();
writeString_P("\n\nRP6Control SPI ACCSENS Test Program!\n");
setLEDs(0b1111);
mSleep(50);
initLCD();
showScreenLCD("################", "################");
showScreenLCD(" SPI ACCSENS", " Test Program");
mSleep(1000);
setLEDs(0b0000);
SPI_ACCSENS_init();
uint8_t whoami = SPI_ACCSENS_readByte(WHO_AM_I);
writeString_P("\nWHO AM I: ");
writeInteger(whoami, HEX);
writeChar('\n');
mSleep(2000);
startStopwatch1();
while(true)
{
if(getStopwatch1() > 1000)
{
SPI_ACCSENS_read();
writeString_P("\n3D Accelerometer SENSOR ->\n");
writeString_P("X-axis: ");
writeInteger(x_axisa, DEC);
writeChar('\n');
writeString_P("Y-axis: ");
writeInteger(y_axisa, DEC);
writeChar('\n');
writeString_P("Z-axis: ");
writeInteger(z_axisa, DEC);
writeChar('\n');
SPI_ACCSENS_normalize();
SPI_ACCSENS_position();
writeString_P("POSITION: \n");
writeString_P(" Pitch [°]: ");
writeDouble(pitch, 6, 1);
writeChar('\n');
writeString_P(" Roll [°]: ");
writeDouble(roll, 6, 1);
writeChar('\n');
setCursorPosLCD(0, 0);
writeStringLCD_P("P");
writeDoubleLCD(pitch, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8);
writeStringLCD_P("R");
writeDoubleLCD(roll, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0);
writeStringLCD_P("X");
writeIntegerLCD(x_axisa, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 5);
writeStringLCD_P("Y");
writeIntegerLCD(y_axisa, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 10);
writeStringLCD_P("Z");
writeIntegerLCD(z_axisa, DEC);
writeStringLCD_P(" ");
mSleep(2000);
}
}
return 0;
}
Erklärung
Erfahrungsberichte
...in Arbeit...(kann aber gerne ergänzt werden)
Siehe auch
- RP6v2
- RP6
- RP6 - Programmierung
- RP6 Kamera - Mitmach-Projekt
- RP6v2 I2C-Portexpander
- RP6v2 USB-RS232-Adapter
- RP6v2 Orientierung
- RP6 Multi IO Projekt
- RP6 Multi IO Projekt - Software
- CCRP5
- Yeti
- Asuro
- C't-Bot
Weblinks
Siehe auch die Weblinks im RP6v2 und im RP6 Artikel!
- RP6 Sensor Board bei CONRAD
- JM3 Engeneering: RP6 Roboter Erweiterung
- Ankündigung einer Sensorerweiterungsplatine für die RP6v2 M256 WiFi im Roboternetz RP6 Forum
- Erfahrungsbericht zum Sensor Board im AREXX Support Forum und im Roboternetz RP6 Forum
- Erfahrungsbericht zum I2C GPS Empfänger im AREXX Support Forum
Autoren
--Dirk 07:07, 30. Apr 2014 (CET)
