NIBO2 - Test Bodensensoren

[Lisbeth2010]

Hier mal ein Screenshot der eingebundenen freeRTOS Dateien.
Und hier der Code der main.c, die hier freeRTOSminimal.c heißt. Das Programm hält an einer schwarzen Linie (Isolierband) an und fährt zurück. Der 2. Task schaltet nur eine LED. Als Zugabe: Binäre Ausgabe einer Zahl zwischen 0 und 63.
Danke auch an cypax.net, der das so oder ähnlich schon mal für den ASURO gemacht hat.
Das hex-File hat 31 kByte.

/* FUNKTIONIERT!!! 09.12.2010 IW*/

#include <avr/interrupt.h>

#include "FreeRTOS.h"
#include "task.h" 

#include <nibo/niboconfig.h>
#include <nibo/iodefs.h>

#include <nibo/bot.h>
#include <nibo/i2cmaster.h>

#include <nibo/leds.h>
#include <nibo/delay.h>
#include <nibo/floor.h>
#include <nibo/adc.h>
#include <nibo/motco.h>

int y = 1;

// a struct to pass parameters to tasks
typedef struct LED_PARAMETERS {
	unsigned int LED;		     //the LED the task should use
	unsigned int FlashRate;	 //the rate at which the LED should flash
} xLEDParameters;

//a simple function to set DDR-registers and initial state of the LEDs
void Init(void);

//this function starts two tasks
void StartLEDFlashTasks(unsigned char uxPriority);

//1st task
void TASK1(void *pvParameters);

//2nd task
void TASK2(void *pvParameters);

// Binäre Ausgabe auf LED
void zahlausgabe(int16_t wert);

// Bodensensoren messen
void floor_measure_iw();

// anhalten an Isolierband
void laufstall(void);

int main(void) {
	Init();

	bot_init();
	i2c_init();
	leds_init();
	floor_init();

	StartLEDFlashTasks (1);
	// Start Scheduler 
	vTaskStartScheduler(); 
	return 0; 
}

void vApplicationIdleHook(void) {
	y++;
}

//A simple function to set DDR-registers and initial state of the LEDs
void Init(void) {
	// Set port E direction to outputs. (LED RED Nibo)
	DDRE = 0xFC;
	PORTE &= ~((1<<PE7) || (1<<PE6)); // aus (Startbedingung)
}

void StartLEDFlashTasks(unsigned char uxPriority){
	//pointer to struct to pass task parameters
  xLEDParameters *pxLEDParameters;
	//FlashRate for 1st task
	const unsigned int FlashRate1 = 2000/portTICK_RATE_MS;
	//FlashRate for 2nd task
	const unsigned int FlashRate2 = 4000/portTICK_RATE_MS;
	
	// Create and complete the structure used to pass parameters to the next Created task. 
	pxLEDParameters = (xLEDParameters *) pvPortMalloc(sizeof(xLEDParameters));
	pxLEDParameters->LED = (1<<PE7); //1st task will use LED on pin PE7
	pxLEDParameters->FlashRate = FlashRate1;
	// Create the task. 
	xTaskCreate(TASK1, (signed char *) "TASK1", configMINIMAL_STACK_SIZE, (void *) pxLEDParameters, uxPriority, (xTaskHandle *) NULL );

	// Create and complete the structure used to pass parameters to the next Created task. 
	pxLEDParameters = (xLEDParameters *) pvPortMalloc( sizeof( xLEDParameters ) );
	pxLEDParameters->LED = (1<<PE6); //2nd task will use LED on pin PE6
	pxLEDParameters->FlashRate = FlashRate2;
	// Create the task. 
	xTaskCreate(TASK2, (signed char *) "TASK2", configMINIMAL_STACK_SIZE, (void *) pxLEDParameters, uxPriority, (xTaskHandle *) NULL );
}
	
void TASK2(void* pvParameters) {
	xLEDParameters *pxParameters;
	pxParameters = (xLEDParameters *) pvParameters;
	while(1) { 
		PORTE = PORTE | (pxParameters->LED);
		vTaskDelay(pxParameters->FlashRate/2);
		PORTE = PORTE & ~(pxParameters->LED);
		vTaskDelay(pxParameters->FlashRate/2); 
	}
} 

void TASK1(void* pvParameters) {
	xLEDParameters *pxParameters;
	pxParameters = (xLEDParameters *) pvParameters;
	
	while(1){ 
		floor_measure_iw(); // schwarz = 0, weiß = 1023 bzw. 63=1023/16 Integer!
		laufstall();
	}
}

/* eigene Funktion Zahlausgabe binär 0..63 */
int16_t wert;
void zahlausgabe(int16_t wert) {
	if ((wert - 32) > 0) {
		leds_set_status(LEDS_GREEN, 5);
		wert = wert - 32;
	} else {
		leds_set_status(LEDS_OFF, 5);
	}
	if ((wert - 16) > 0) {
		leds_set_status(LEDS_GREEN, 4);
		wert = wert - 16;
	} else {
		leds_set_status(LEDS_OFF, 4);
	}
	if ((wert - 8) > 0) {
		leds_set_status(LEDS_GREEN,3);
		wert = wert - 8;
	} else {
		leds_set_status(LEDS_OFF,3);
	}
	if ((wert - 4)> 0) {
		leds_set_status(LEDS_GREEN,2);
		wert = wert - 4;
	} else {
		leds_set_status(LEDS_OFF,2);
	}
	if ((wert - 2)> 0) {
		leds_set_status(LEDS_GREEN,1);
		wert = wert - 2;
	} else {
		leds_set_status(LEDS_OFF,1);
	}
	if ((wert - 1)> 0) {
		leds_set_status(LEDS_GREEN,0);
	} else {
		leds_set_status(LEDS_OFF,0);
	}
} // end Zahlausgabe

/* eigene Funktion floor_measure() 2x wg. erster AD-Wandlung */
int16_t floor_mw[4]; 
void floor_measure_iw() {
	floor_enable_ir();
	floor_mw[0] = 1023-adc_read(0); // Bodensensor von oben in Fahrtrichtung rechts
  floor_mw[1] = 1023-adc_read(1);	// Bodensensor von oben in Fahrtrichtung links
  floor_mw[2] = 1023-adc_read(2);	// Liniensensor von oben in Fahrtrichtung links
  floor_mw[3] = 1023-adc_read(3);	// Liniensensor von oben in Fahrtrichtung rechts
	//nochmal, wir haben ja Zeit...
	floor_mw[0] = 1023-adc_read(0); // Bodensensor von oben in Fahrtrichtung rechts
  floor_mw[1] = 1023-adc_read(1);	// Bodensensor von oben in Fahrtrichtung links
  floor_mw[2] = 1023-adc_read(2);	// Liniensensor von oben in Fahrtrichtung links
  floor_mw[3] = 1023-adc_read(3);	// Liniensensor von oben in Fahrtrichtung rechts
	floor_disable_ir();
}

/*-----------*/
/* Laufstall */
/*-----------*/

uint8_t		speed_left	= 20;	// Ticks/Sekunde Testwert
uint8_t		speed_right	= 20;	// Ticks/Sekunde Testwert
uint16_t		taskdelay		= 2000;
uint16_t	floor_left;
uint16_t	floor_right;

void laufstall() {
	floor_left = floor_mw[1]/16;
	floor_right = floor_mw[0]/16;
	//zahlausgabe(floor_right); // da kein Display! keine negativen Werte!	
	// floor_left  auf gelb: 7, auf schwarz: 0, auf Fliese 6, auf Fuge 3 
	// floor_right auf gelb: 8, auf schwarz: 0, auf Fliese 6, auf Fuge 3  
	if ((floor_left < 2) || (floor_right < 2)) { //ABGRUND: STOP und zurück
		leds_set_status(LEDS_ORANGE,0);
		leds_set_status(LEDS_ORANGE,5);
		motco_stop();
		motco_update();
		vTaskDelay(taskdelay);
		motco_setSpeed(-speed_left/2, -speed_right/2);
		motco_update();
		vTaskDelay(taskdelay);
		motco_stop();
		motco_update();
	} else {
		leds_set_status(LEDS_GREEN,0);
		leds_set_status(LEDS_GREEN,5);
		motco_setSpeed(speed_left, speed_right);
		motco_update();
	}
} // end laufstall

Für Nibo2 muß motco durch copro ersetzt werden.
Die Lisbeth2010