Hi...hier bin ich mal wieder. Da die Noten für das Projekt nun fest stehen kann ich nun den Code posten.
Code:
/***************************Bibliotheken einbinden*****************************/
#include "asuro.h"
#include "myasuro.h"
/*******************Defines und Variablen für Fahren Regelung******************/
unsigned int data[2]; //Speicherplatz für Odometriedaten bereitstellen
signed int status[2]={0,0}; // Variable zur Bestimmung der Hell/Dunkel wechsel
signed int difference=0; // Variable zur regelung der Motorgeschwindigkeit
#define TRIGGERLEVEL 650 //Schwellwert für Hell/Dunkel übergänge
#define HYSTERESIS 10 // Ausgleich von Messfehlern
#define LOW 0 // Definition für Dunkel
#define HIGH 1 // Definition für Hell
#define keine_taste (PINC & (1<<PC4)) //Taster direkt abfragen
/************Variablen für Zeitvorgaben zwischen den Kollisionen***************/
unsigned long meineZeit;
unsigned long meineZeit2;
/**************Speicherplatz für Liniendaten bereitstellen*********************/
unsigned int umgebung[1];
/***Variable für Dunkelerkennung, damit der Asruro im Dunkeln stehen bleibt****/
int dunkel;
/**************************Vorwärts fahren*************************************/
void Fahren(void)
{
// Prozessor neu initialieren um EncoderInit() zurückzusetzen
// da sonst die Regelung nicht mehr funktioniert
Init();
FrontLED(ON);
;
//Motor Drehrichtung bestimmen
MotorDir(FWD, FWD);
// so lange gerade aus fahren bis ein Taster gedrückt wird
// und es hell genug ist
while((keine_taste) && (umgebung[0]>1))
{
LineData(umgebung);
// Helligkeitswerte der Lichtschranken auslesen
OdometrieData(data);
// Wechsel linker Sensor von niedrig auf hoch?
if ((status[0]==LOW) && (data[0]>TRIGGERLEVEL+HYSTERESIS)) {
status[0]=HIGH;
difference++;
}
// Wechsel linker Sensor von hoch auf niedrig?
if ((status[0]==HIGH) && (data[0]<TRIGGERLEVEL-HYSTERESIS)) {
status[0]=LOW;
difference++;
}
// Wechsel rechter Sensor von niedrig auf hoch?
if ((status[1]==LOW) && (data[1]>TRIGGERLEVEL+HYSTERESIS)) {
status[1]=HIGH;
difference--;
}
// Wechsel rechter Sensor von hoch auf niedrig?
if ((status[1]==HIGH) && (data[1]<TRIGGERLEVEL-HYSTERESIS)) {
status[1]=LOW;
difference--;
}
// zur Sicherheit: verhindern, dass der Differenzzäler
// den erlaubten Wertebereich verlässt
if (difference<-255) difference=-255;
if (difference>255) difference=255;
// Status-LED noch entsprechend der erkannten Segmente
// aufleuchten lassen, grun für links, rot für rechts
StatusLED(status[0]+status[1]*2);
// Zäldifferenz passend auf die Motoren verteilen
if (difference>0) MotorSpeed(190-difference,190);
else MotorSpeed(190,190+difference);
}
Dunkel ();
}
/********Einzelfunktionen für Verarbeitung in First-,Second und Thirdcol********/
void Anhalten(void) /* Anhalten */
{
StatusLED(RED);
BackLED(OFF,OFF);
MotorDir(BREAK,BREAK);
}
void Zurueck(void) /* 20mm Zurück fahren */
{
EncoderInit();
GoTurn(-40,0,200);
}
void KurveR(void) /* 90° Kurve Rechts */
{
EncoderInit();
GoTurn(0,90,200);
}
void KurveL(void) /* 90° Kurve Links */
{
EncoderInit();
GoTurn(0,-90,200);
}
void Drehung(void) /*180° Drehung*/
{
EncoderInit();
GoTurn(0,180,200);
}
/****************Programm Rechtskurve im Parcour 1. Kollision******************/
void Firstcol(void)
{
Anhalten();
Msleep(250);
Zurueck();
Anhalten();
Msleep(10);
KurveR();
Anhalten();
Msleep(250);
meineZeit=Gettime(); //Aktuelle Prozessorzeit für Kollisionserkennung
//in meineZeit speichern
switched = 0;
StartSwitch ();
}
/*****************Programm Linkskurve im Parcour, 2. Kollision*****************/
void Secondcol(void)
{
Anhalten();
Msleep(250);
Zurueck();
Anhalten();
Msleep(250);
Drehung();
Anhalten();
Msleep(250);
meineZeit2=Gettime(); //Aktuelle Prozessorzeit für Kollisionserkennung
// in meineZeit2 speichern
switched = 0; // Kollisionserkennung auf 0 setzen
StartSwitch(); // Taster-Interrupt aktivieren
}
/*****************Programm Sackgasse im Parcour, 3. Kollision******************/
void Thirdcol(void)
{
Anhalten();
Msleep(250);
Zurueck();
KurveL();
Anhalten();
Msleep(250);
switched = 0; // Kollisionserkennung auf 0 setzen
StartSwitch(); // Taster-Interrupt aktivieren
}
/******Abfrage der Liniensensoren um Helligkeit der Umgebung festzustellen*****/
void Dunkel (void)
{
LineData(umgebung);
if (umgebung[0]<1)
{
dunkel = 1;
}
else if(umgebung[0]>1)
{
dunkel = 0;
}
}
/*****Wird ausgeführt, wenn der Asuro das ende des Labyrinths erreicht hat!****/
/*******Ende des Labyrints muss durch eine Einhausung abgedunkelt werden!******/
void Ziel(void)
{
int i;
for(i = 0; i < 16; i++) /* Asuro steht im Dunkeln, Ziel erreicht*/
{
MotorDir(FWD,FWD);
MotorSpeed(50,50);
BackLED(OFF,OFF);
StatusLED(OFF);
FrontLED(OFF);
Msleep(200);
MotorDir(BREAK,BREAK);
BackLED(ON,ON);
StatusLED(RED);
FrontLED(ON);
Msleep(200);
}
dunkel=0;
}
/*****************************Hauptprogrammbeginn******************************/
int main (void)
{
Init (); // Prozessor initialisieren
StatusLED(RED); // 3 Sekunden Countdown zum Start,
Msleep(1000);
StatusLED(YELLOW); // Status Led wird im Sekundentakt
Msleep(1000);
StatusLED(GREEN); // von Rot über Gelb zu Grün geschaltet
Msleep(1000);
FrontLED(ON);
switched = 0; // Kollisionserkennung auf 0 setzen
StartSwitch (); // Taster-Interrupt aktivieren
/********************Endlosschleife für Zustandsabfrage************************/
while(1)
{
/******************Wenn 1. Kollision, dann Rechtskurve 90°*********************/
if ((switched == 1)&&(Gettime()>meineZeit+1000)&&(Gettime()>meineZeit2+600))
{
Firstcol();
}
/*********Wenn 2. Kollision innerhalb 1s nach abarbeitung von Firstcol*******/
else if ((switched == 1)&&(Gettime()<meineZeit+1000))
{
Secondcol();
}
/********Wenn 3. Kollision innerhalb 0,6s nach abarbeitung von Secondcol*******/
else if ((switched == 1)&&(Gettime()<meineZeit2+600))
{
Thirdcol();
}
/**************************Wenn umgebung zu dunkel*****************************/
else if(dunkel==1)
{
Ziel();
}
/*********************Wenn keine Kollision, Fahre normal***********************/
else
{
Fahren();
}
}
/*********************Endlosschleife - Ende des Programmes*********************/
/*****Um zu verhindern, dass eventuell alte Programmteile aufgerufen werden****/
/*********************die sich noch im Speicher befinden***********************/
while(1);
return 0;
}
Folgende Dinge müsst ihr beim Bau des Labyriths beachten!!!
1. Es muss rechtwinklig aufgebaut sein
2. Bei Sackgassen muss der Lösungsweg gegenüber der Sackgasse liegen
Bei Meinem Asuro hab ich folgendes verbessert:
1. Odometriesensoren abgedunkelt mit schwarzem Papier
2. Die Odometriescheiben(12er) habe ich mit hilfe von Kabelisolierung, die ich über die Welle geschoben habe befestigt um den Abstand zu den Sensoren gleich zu halten.
Hab euch noch 2 Bilder von den Verbesserungen und das .hex file gepostet und ein kleines Video bei Youtube hoch geladen.
Videolink: http://www.youtube.com/watch?v=JB6F8HWTHLE
So nun wünsch ich euch viel Spass und Erfolg mit dem Code.
P.S. Das Programm funktioniert nur mit der aktuellen, erweiterten Lib aus dem Roboternetz Forum.
Ein großes danke Schön an die, die die Lib entwickeln, einfach spitze was ihr da mittlerweile auf die Beine gestellt habt.
Gruß
G3tzR@zor
Zitieren
Lesezeichen