naja
es ist das beispielprogramm i2c_slave von der lib des rp6
und es ist das beispiel prgramm movement2 vom c controller
Movement:
Code:
/*******************************************************************************
* RP6 C-Control PRO M128 Erweiterungsmodul
* ----------------------------------------------------------------------------
* I2C Bus - Bewegung & Sensorik - Beispielprogramm
* ----------------------------------------------------------------------------
* Ein weiteres einfaches Bewegungsprogramm, diesmal reagiert der RP6
* allerdings auf die Bumper.
* Zunächst fährt der RP6 vorwärts in einer Kreisbahn.
* Kollidiert er mit einem Hindernis, werden die Motoren gestoppt und
* die LEDs fangen an zu blinken.
* (man könnte natürlich auch anders auf die Kollision reagieren...)
******************************************************************************/
#include "../../RP6CCLib/RP6CCLib.cc"
// ###########################
#include "RP6MasterLib.cc"
// ###########################
#include "RP6_Sensorcheck.cc"
/**
* Dieser Thread wird ausgeführt sobald der Roboter mit einem
* Hindernis kollidiert ist.
*/
void blink(void)
{
while(true)
{
setRP6LEDs(LED2 | LED3);
setLEDs(LED3 | LED4 | LED5); // LEDs kurz aufblitzen lassen ...
Thread_Delay(5); // ...
setLEDs(0); // ...
Thread_Delay(30);
setRP6LEDs(LED5 | LED6);
setLEDs(LED3 | LED4 | LED5);
Thread_Delay(5);
setLEDs(0);
Thread_Delay(30);
}
}
void main(void)
{
RP6_CCPRO_Init(); // Auf Startsignal warten!
newline();
println(" ________________________");
println(" \\| RP6 ROBOT SYSTEM |/");
println(" \\_-_-_-_-_-_-_-_-_-_/ ");
newline();
println(" RP6 Movement DEMO 2 ");
newline();
beep(100, 200, 300);
showScreenLCD("Movement", "DEMO Program 2");
AbsDelay(2000);
clearLCD();
// ACS auf mittlere Sendeleistung stellen:
RP6_writeCMD_1param(RP6_BASE_ADR, CMD_SET_ACS_POWER, ACS_PWR_MED);
RP6_writeCMD_1param(RP6_BASE_ADR, CMD_SET_WDT, true);
RP6_writeCMD_1param(RP6_BASE_ADR, CMD_SET_WDT_RQ, true);
// Der Thread zur Überprüfung von Statusänderungen wird gestartet:
Thread_Start(1,thread_checkINT0);
// Vorwärts im Kreis fahren:
changeDirection(FWD);
moveAtSpeed(100,40);
setCursorPosLCD(1,0);
printLCD("Moving... :-)");
setRP6LEDs(LED1 | LED4);
byte stopped;
stopped = false;
while(true)
{
if(!stopped) { // Es sollte nur einmal stop() und Thread_Start aufgerufen
// werden, daher fragen wir hier ab, ob das bereits
// geschehen ist.
// Man muss die Bumper nicht in RP6_Sensorcheck.cc
// auswerten. Man kann auch hier direkt die beiden Variablen
// der Tastsensoren abfragen:
if(bumperLeft || bumperRight)
{
stop(); // Roboter stoppen!
Thread_Start(2,blink); // Blinken!
stopped = true; // Roboter wurde angehalten!
setCursorPosLCD(1,0);
printLCD("Ouch! Crash! :-(");
}
}
Thread_Delay(1); // Muss immer hier rein, sonst werden die anderen
// Threads blockiert.
}
}
sensorcheck:
Code:
/*******************************************************************************
* RP6 C-Control PRO M128 Erweiterungsmodul
* ----------------------------------------------------------------------------
* I2C Bus Beispielprogramm
* ----------------------------------------------------------------------------
* In diese Datei wurde die Sensorauswertung ausgelagert. So bleibt
* das Programm insgesamt übersichtlicher.
*
* Die Ausgaben des ACS wurden in diesem Beispiel entfernt!
******************************************************************************/
//------------------------------------------------------------------------------
//
#ifndef __SENSORCHECK__ // Alles nur EINMAL einbinden!
#define __SENSORCHECK__
// Diese zwei Zeilen und die letzte in dieser Datei sind wichtig, sonst
// funktioniert das übersetzen nicht! Auch bei eigenen Erweiterungen MUSS das
// am Anfang stehen!
/**
* Wenn sich der Zustand des ACS verändert hat, wird diese Funktion
* aufgerufen. Was hier genau gemacht wird, hängt dann von der Anwendung ab.
*/
void handle_ACS(void)
{
}
/**
* Genauso wie bei der ACS Funktion, wird handle_Bumpers aufgerufen sobald
* sich der Zustand der Tastsensoren vom RP6 geändert hat.
*/
void handle_Bumpers(void)
{
beep_t(100, 1, 1);
if(bumperRight) { // Rechter Tastsensor
setCursorPosLCD(0,14);
printLCD("BR");
}
else // Links nichts detektiert - also Text vom LCD löschen:
clearPosLCD(0,14,2);
if(bumperLeft) { // Linker Tastsensor
setCursorPosLCD(0,0);
printLCD("BL");
}
else
clearPosLCD(0,0,2);
if(bumperLeft && bumperRight) { // Beide, also in der Mitte
setCursorPosLCD(0,6);
printLCD("MID");
}
else
clearPosLCD(0,6,3);
}
/**
* Bei niedriger Akkuspannung wird diese Funktion aufgerufen
*/
void handle_BatLow(void)
{
print("BAT LOW:");
printInteger(adcBat);
println(" ");
}
/**
* Sobald Bewegungsabläufe abgeschlossen sind, oder bei einer
* Fehlerabschaltung aufgrund blockierter Motoren wird diese Funktion
* aufgerufen.
*/
void handle_DriveSystemChange(void)
{
print(" DRIVE:");
printInteger(movementComplete);
printInteger(motorOvercurrent);
println(" ");
}
/**
* Der RP6 kann mit dem IR Empfänger vom ACS auch RC5 Codes von Fernbedienungen
* empfangen. Sobald ein Tastencode empfangen wurde, wird diese Funktion
* aufgerufen.
*/
void handle_RC5_reception(void)
{
beep_t((RC5_data+5)*15, 2, 2);
print(" RC5: ");
printInteger(RC5_toggle); // Togglebit (Taste gehalten
print(":"); // oder mehrfach gedrückt?)
printInteger(RC5_adr); // Adresse
print(":");
printInteger(RC5_data); // Tastencode
println(" ");
}
/**
* Damit der Roboter nicht ausser Kontrolle gerät, wenn das CCPRO oder andere
* Host Controller abstürzen (oder die Kommunikation aus sonstigen Gründen
* gestört ist), kann ein Watchdog Timer aktiviert werden, der
* alle 250ms eine Anfrage an den Host sendet. Antwortet dieser nicht innerhalb
* von 3 Sekunden auf diese Anfrage, werden die Motoren gestoppt und der Roboter
* muss zurückgesetzt werden bevor er weiterfahren kann.
*
* Man kann diese periodischen Anfragen auch gut verwenden um zu signalisieren
* dass das Programm noch läuft, indem man eine LED blinken lässt, oder wie
* in diesem Beispielprogramm ein Zeichen auf dem LC-Display ("Heartbeat").
*/
byte heartbeat;
void handle_WDT_RQ(void)
{
if(heartbeat)
{
clearPosLCD(0, 11, 1);
heartbeat = false;
}
else
{
setCursorPosLCD(0,11);
printLCD("*");
heartbeat = true;
}
}
/**
* Dies ist die etwas aufgeräumte Interrupt Behandlungsroutine. Die eigentliche
* Auswertung der Daten erfolgt in den einzelnen Funktionen oben. Diese
* werden hier bei Bedarf aufgerufen.
*/
void thread_checkINT0(void)
{
while(true) {
if(Port_ReadBit(PORT_PE5_INT)) { // Hat der Controller auf dem Mainboard neue Daten?
// Die drei Status Register auslesen:
RP6_readRegisters(RP6_BASE_ADR, 0, messageBuf, 3);
// Wir überprüfen, ob die Status Bits sich verändert haben,
// also verknüpfen wir den alten und neuen Wert per Exclusiv Oder
// um nur die geänderten Bits auswerten zu müssen.
byte compare;
compare = messageBuf[0] ^ interrupt_status;
interrupt_status = messageBuf[0];
// ------------------------------------
// Zunächst prüfen wir ob sich die ACS Status Bits verändert haben.
// Falls nicht, hat irgend etwas anderes den Interrupt ausgelöst und
// wir können mit den anderen Abfragen weitermachen.
if(compare & MASK_ACS)
{
// Die ACS Status Variablen setzen:
obstacleLeft = messageBuf[0] & 0x20;
obstacleRight = messageBuf[0] & 0x40;
// ACS in getrennter Funktion weiterbehandeln:
handle_ACS();
}
// ------------------------------------
// Abfragen ob sich der Zustand der Bumper geändert hat, falls
// ja, Text auf der seriellen Schnittstelle ausgeben:
if(compare & MASK_BUMPER)
{
bumperLeft = messageBuf[0] & 0x2;
bumperRight = messageBuf[0] & 0x4;
handle_Bumpers();
}
// ------------------------------------
// Überprüfen ob ein RC5 Datenpaket mit dem IRCOMM empfangen
// worden ist - falls ja auf der seriellen ausgeben:
if(interrupt_status & MASK_RC5)
{
RP6_readRegisters(RP6_BASE_ADR, I2C_REG_RC5_ADR, messageBuf, 2);
RC5_toggle = messageBuf[0] >> 5;
RC5_adr = messageBuf[0] & 0x1F;
RC5_data = messageBuf[1];
handle_RC5_reception();
}
// ------------------------------------
// Überprüfen ob die Batteriespannung zu niedrig ist:
if(compare & MASK_BATLOW)
{
RP6_readRegisters(RP6_BASE_ADR, I2C_REG_ADC_UBAT_L, messageBuf, 2);
adcBat = (messageBuf[1] << 8) | (messageBuf[0]);
handle_BatLow();
}
// ------------------------------------
// Bewegung fertig? Notabschaltung?
if(compare & MASK_DRIVE)
{
movementComplete = messageBuf[2] & 1;
motorOvercurrent = messageBuf[2] & 4;
handle_DriveSystemChange();
}
// ------------------------------------
// Auf Watchdog Requests prüfen:
if(messageBuf[1] & MASK_WDT_RQ)
{
handle_WDT_RQ();
}
}
Thread_Delay(1);
}
}
#endif
Zitieren
Berechtigungen
Lesezeichen