Code:
#include <stdlib.h>
#include <avr/interrupt.h>
#include "nibo/niboconfig.h"
#include "nibo/iodefs.h"
#include "nibo/delay.h"
#include "nibo/adc.h"
#include "nibo/pwm.h"
#include "nibo/i2cmaster.h"
#include "nibo/display.h"
#include "nibo/bot.h"
#include "nibo/leds.h"
#include "nibo/gfx.h"
#include "nibo/irco.h"
#include "nibo/motco.h"
#include "nibo/floor.h"
#define LINKS 0
#define VORNE_LINKS 1
#define VORNE 2
#define VORNE_RECHTS 3
#define RECHTS 4
#define SPEEDFACTOR 30
// Zustände
#define BLOCKIERT 1
#define AUSWEICHEN 2
#define FREI 0
#define HINDERNISLINKS 3
#define HINDERNISRECHTS 4
#define GERADEAUS 5
// Deklarationen von Hilfsfunktionen
void Init();
void float2string(float value, int decimal, char* valuestring);
void leds_set_status_all(uint8_t col0, uint8_t col1, uint8_t col2, uint8_t col3, uint8_t col4, uint8_t col5);
float SupplyVoltage(void);
void textout(int x, int y, char* str, int ft);
int main()
{
Init();
// Kollisionsvermeidung vorbereiten
uint16_t Vektor[5][2]; // Einheitsvektoren (*10) [0] ist x- und [1] ist y-Wert
Vektor[0][0] = -10; // LINKS x
Vektor[0][1] = 0; // LINKS y
Vektor[1][0] = -7; // VORNE_LINKS x
Vektor[1][1] = 7; // VORNE_LINKS y
Vektor[2][0] = 0; // VORNE x
Vektor[2][1] = 10; // VORNE y
Vektor[3][0] = 7; // VORNE_RECHTS x
Vektor[3][1] = 7; // VORNE_RECHTS y
Vektor[4][0] = 10; // RECHTS x
Vektor[4][1] = 0; // RECHTS y
uint8_t weightfactor[5]; // Gewichtungsfaktor
weightfactor[LINKS] = 1;
weightfactor[VORNE_LINKS] = 2;
weightfactor[VORNE] = 3;
weightfactor[VORNE_RECHTS] = 2;
weightfactor[RECHTS] = 1;
uint16_t VektorMalSensor[5][2]; // Sensorwert * Einheitsvektor (*10)
uint16_t VektorMalSensorSumme[2]; // Sensorschwerpunkt (x,y) für Auswertung
// Vorbereitungen
leds_set_displaylight(1000);
leds_set_headlights(256);
floor_enable_ir();
motco_setPWM(512,512);
motco_setSpeed(3,3);
// fixe Display-Anzeigen
textout(35,0,"Volt", 0);
textout(0, 8,"distance:", 0);
textout(0,24,"floor:", 0);
textout(0,40,"line:", 0);
// Hauptschleife
while(1)
{
// Akkuspannung anzeigen
float Ubatt = SupplyVoltage();
char text[6];
float2string(Ubatt,2,text);
textout(0,0," ",0); // 5 Zeichen löschen
textout(0,0,text, 0);
// Abstandsmessung Raumgefühl
irco_startMeasure();
irco_update();
// Floor
uint16_t floor_distance[2];
uint16_t line_distance[2];
// Abstandsmessung Floor
floor_update();
floor_distance[0] = floor_l;
floor_distance[1] = floor_r;
line_distance[0] = line_l;
line_distance[1] = line_r;
//Strings für Display
char irco_string[5][5];
char floor_string[2][5];
char line_string[2][5];
// Laufvariablen
int i,j;
/*
IR-Abstandssensoren
*/
for(i=0; i<5; ++i)
textout(i*21,16," ",0); //löschen
for(i=0; i<5; ++i) // z.Z. noch rechts 0 und links 4 !!!!!!!!!!!!!
{
itoa(irco_distance[i],irco_string[i],10);
textout(i*21,16,irco_string[i],0);
}
/*
IR-Floorsensoren (Abgrunderkennung)
*/
for(i=0; i<2; ++i)
textout(i*28,32," ",0); //löschen
for(i=0; i<2; ++i)
{
itoa(floor_distance[i],floor_string[i],10);
textout(i*28,32,floor_string[i],0);
}
/*
IR-Liniensensoren
*/
for(i=0; i<2; ++i)
textout(i*28,48," ",0); //löschen
for(i=0; i<2; ++i)
{
itoa(line_distance[i],line_string[i],10);
textout(i*28,48,line_string[i],0);
}
/*
MOTCO
Mathematische Methode "x/y-Schwerpunkt der Sensorvektoren bilden":
(Einheitsvektoren * 10) * Sensorwert (0-255) * weightfactor, davon Summe bilden
VektorMalSensorSumme[...] 0 ist x-Wert und 1 ist y-Wert
Blockade: y kann maximal 14790 groß werden (vl, v, vr 255)
Richtung: x kann maximal -6120 (Hindernis links) bzw. +6120 (H. rechts) werden (l, vl 255 bzw. r, vr 255)
*/
// Ermittlung von VektorMalSensorSumme[...] (gewichteter x- und y-Wert)
VektorMalSensorSumme[0] = 0; // x-Wert
VektorMalSensorSumme[1] = 0; // y-Wert
// i entspricht links, vornelinks, vorne, vornerechts, rechts
// j entspricht x und y
for (i=0; i<5; ++i)
{
for (j=0; j<2; ++j)
{
VektorMalSensor[i][j] = Vektor[i][j] * irco_distance[i] * weightfactor[i]; // 4-i wegen IRCo?????
VektorMalSensorSumme[j] += VektorMalSensor[i][j];
}
}
// Reaktion auf VektorMalSensorSumme[...] (x- und y-Wert)
// GrenzenY
uint16_t GrenzeY1 = 12000; // Zustandsgrenze: BLOCKIERT / AUSWEICHEN
uint16_t GrenzeY2 = 6000; // Zustandsgrenze: AUSWEICHEN / FREI
// GrenzenX
uint16_t GrenzeXlinks = -2000; // Zustandsgrenze: LINKS / GERADEAUS
uint16_t GrenzeXrechts = 2000; // Zustandsgrenze: RECHTS / GERADEAUS
// Zustandsvariable
uint8_t zustand = 0;
uint8_t zustand_old = 0;
// Zustand ermitteln
{ // y-Wert
if( VektorMalSensorSumme[1] >=GrenzeY1) zustand = BLOCKIERT;
if((VektorMalSensorSumme[1] < GrenzeY1) &&
(VektorMalSensorSumme[1] >=GrenzeY2))
{
// x-Werte
if( VektorMalSensorSumme[0] < GrenzeXlinks ) zustand = HINDERNISLINKS;
if( VektorMalSensorSumme[0] > GrenzeXrechts ) zustand = HINDERNISRECHTS;
if((VektorMalSensorSumme[0] >=GrenzeXlinks) &&
(VektorMalSensorSumme[0] <=GrenzeXrechts)) zustand = GERADEAUS;
}
if (VektorMalSensorSumme[1] < GrenzeY2) zustand = FREI;
}
// Auf Zustand reagieren
if(zustand == zustand_old)
{
// kein MOTCo-Befehl notwendig
}
else //Veränderung eingetreten
{
// Sondermaßnahmen
// gegen Schwingung links/rechts: einmal GERADEAUS erzwingen
if((zustand_old == HINDERNISLINKS) || (zustand_old == HINDERNISRECHTS))
{
zustand = GERADEAUS;
}
// gegen Schwingung vor/zurück: zweimal zurück
if((zustand_old == BLOCKIERT) && (zustand == GERADEAUS))
{
zustand = BLOCKIERT;
}
// direkt vorne frei?
if(irco_distance[2]<150)
{
zustand = zustand_old;
}
//Allgemeine Maßnahmen
switch(zustand)
{
case FREI:
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_GREEN, LEDS_GREEN, LEDS_OFF, LEDS_OFF);
//do
motco_setSpeed( 3*SPEEDFACTOR, 3*SPEEDFACTOR ); // rasch vorwärts
delay(10);
//exit
break;
case HINDERNISRECHTS:
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_ORANGE, LEDS_ORANGE);
//do
motco_setSpeed( -SPEEDFACTOR, SPEEDFACTOR ); // nach links drehen
delay(10);
//exit
break;
case GERADEAUS:
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_ORANGE, LEDS_ORANGE, LEDS_OFF, LEDS_OFF);
//do
motco_setSpeed( 2*SPEEDFACTOR, 2*SPEEDFACTOR ); // gemäßigt vorwärts
delay(10);
//exit
break;
case HINDERNISLINKS:
//entry
leds_set_status_all(LEDS_ORANGE, LEDS_ORANGE, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF);
//do
motco_setSpeed( SPEEDFACTOR, -SPEEDFACTOR ); // nach rechts drehen
delay(10);
//exit
break;
case BLOCKIERT:
//entry
leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_RED, LEDS_RED, LEDS_OFF, LEDS_OFF);
//do
motco_setSpeed(-2*SPEEDFACTOR,-2*SPEEDFACTOR ); // rückwärts fahren
delay(10);
//exit
break;
}
zustand_old = zustand;
motco_update();
}
}//Ende while-Hauptschleife
while(1);
return 0;
}
// Hilfsfunktionen
void Init()
{
sei(); // enable interrupts
i2c_init();
pwm_init();
display_init();
bot_init();
leds_init();
floor_init();
gfx_init();
}
void leds_set_status_all(uint8_t col0, uint8_t col1, uint8_t col2, uint8_t col3, uint8_t col4, uint8_t col5)
{
leds_set_status(col0,0);
leds_set_status(col1,1);
leds_set_status(col2,2);
leds_set_status(col3,3);
leds_set_status(col4,4);
leds_set_status(col5,5);
}
float SupplyVoltage(void)
{
bot_update();
return(0.0166 * bot_supply - 1.19);
}
void textout(int x, int y, char* str, int ft)
{
gfx_move(x,y);
gfx_print_text(str,ft);
}
void float2string(float value, int decimal, char* valuestring)
{
int neg = 0; char tempstr[20];
int i = 0; int j = 0; int c; long int val1, val2;
char* tempstring;
tempstring = valuestring;
if (value < 0){ neg = 1; value = -value; }
for (j=0; j < decimal; j++) {value = value * 10;}
val1 = (value * 2);
val2 = (val1 / 2) + (val1 % 2);
while (val2 !=0){
if ((decimal > 0) && (i == decimal)){
tempstr[i] = (char)(0x2E);
i++;
}
else{
c = (val2 % 10);
tempstr[i] = (char) (c + 0x30);
val2 = val2 / 10;
i++;
}
}
if (neg){
*tempstring = '-';
tempstring++;
}
i--;
for (;i > -1;i--){
*tempstring = tempstr[i];
tempstring++;
}
*tempstring = '\0';
}
Fehlermelungen:
../beginn1.c: In function 'main':
../beginn1.c:76: warning: implicit declaration of function 'motco_setPWM'
../beginn1.c:77: warning: implicit declaration of function 'motco_setSpeed'
../beginn1.c:96: warning: implicit declaration of function 'irco_startMeasure'
../beginn1.c:97: warning: implicit declaration of function 'irco_update'
../beginn1.c:127: error: 'irco_distance' undeclared (first use in this function)
../beginn1.c:127: error: (Each undeclared identifier is reported only once
../beginn1.c:127: error: for each function it appears in.)
../beginn1.c:282: warning: implicit declaration of function 'motco_update'
../beginn1.c: In function 'textout':
../beginn1.c:326: error: too many arguments to function 'gfx_print_text'
make: *** [beginn1.o] Error 1
Build failed with 4 errors and 5 warnings...
kann mir bitte sagen was ich falsch mache...
Zitieren
Berechtigungen
Lesezeichen