Roboterbausatz Nibo

**ehenkes** · 22.07.2007, 01:57

Mit diesem Programm kann man die gesamte Sensorik des Nibo in Ruhe analysieren (Batteriespannung, IR-Abstand, Abgrundsensoren, Liniensensoren)

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"

void Init(void)
{
    sei(); // enable interrupts

    i2c_init();
    pwm_init();
    display_init();
	
    bot_init();
    leds_init();
    floor_init();
    gfx_init();
}

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';
}

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);
}



int main()
{
	Init();
  
        leds_set_displaylight(1000);
	floor_enable_ir();

        while(1)
	{
	    float Ubatt = SupplyVoltage();
            char text[6];
            float2string(Ubatt,2,text);      

            textout( 0,0,text,  0); 
	    textout(35,0,"Volt",0);

	    textout(0,8,"distance:",0);
	    textout(0,24,"floor:",0);
	    textout(0,40,"line:",0);


	    irco_update();
    	    irco_startMeasure();
	    irco_update();
		

	    uint16_t floor_distance[2];
	    uint16_t line_distance[2];
		
	    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];
    	
		int i;
	
		/*
			IR-Abstandssensoren
		*/
		
		for(i=0; i<5; ++i)
    	       {
     		textout(i*21,16,"      ",0); //löschen
	    }

		for(i=0; i<5; ++i)
    	{
     		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);
	    }

		delay(100);
	}

	while(1);
    return 0;
}

Damit bewegt er sich so, dass er Hindernissen ausweicht:

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

void Init()
{
    sei(); // enable interrupts

    i2c_init();
	pwm_init();
	display_init();
	
	bot_init();
	leds_init();
	floor_init();
    gfx_init();
}

void float2string(float value, int decimal, char* valuestring);

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);
}


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

    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(512); 
	floor_enable_ir();
	motco_setPWM(600,600);
	motco_setSpeed(25,25);

	// 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_update();
		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)
    	{
     		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), davon Summe bilden

			VektorMalSensorSumme[...] 0 ist x-Wert und 1 ist y-Wert
			Blockade: y kann maximal 6120 groß werden (vl, v, vr 255)
			Richtung: x kann maximal -4335 (Hindernis links) bzw. +4335 (H. rechts) werden (l, vl 255 bzw. r, vr 255)
		*/
		
        VektorMalSensorSumme[0] = 0; // x-Wert
        VektorMalSensorSumme[1] = 0; // y-Wert

	    for (i=0; i<5; ++i)
	    {
	        for (j=0; j<2; ++j)
	        {
	            VektorMalSensor[i][j] = Vektor[i][j] * irco_distance[i];
	            VektorMalSensorSumme[j] += VektorMalSensor[i][j];
	        }
	    }

		// Reaktion auf VektorMalSensorSumme[...] (x- und y-Wert) 		
		if(VektorMalSensorSumme[1] >= 4590)
		{
			motco_setSpeed(-25,-25); //rückwärts fahren	
		}
		else if( (VektorMalSensorSumme[1] < 4900) && (VektorMalSensorSumme[1] >= 3060) )
		{
			if(VektorMalSensorSumme[0] < -2500) // Hindernis links
				motco_setSpeed(25,-25);	
			else if ( (VektorMalSensorSumme[0] >= -2500) && (VektorMalSensorSumme[0] <= 2500) ) // kein Hindernis
				motco_setSpeed(25,25);	
			else // Hindernis rechts
				motco_setSpeed(-25,25);	
		}
		else
		{
				motco_setSpeed(30,30);	
		}

		motco_update();
	}

	while(1);
    return 0;
}


// Hilfsfunktionen

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';
}

Thema: Roboterbausatz Nibo

Themen-Optionen

Thema durchsuchen

Anzeige

Baum-Darstellung

Lesezeichen

Lesezeichen

Berechtigungen