Durch einen Umgebungsscan die neue Zielrichtung finden

[WarChild]

Ich hatte jetzt noch eine Idee, um die Messfehler zu umgehen...

Wenn das ACS auslöst, kommt der Messwert dieser Himmelsrichtung ja logischerweise nicht mehr als neue Fahrtrichtung in Frage. Also werden alle Messwerte während das ACS auslöst mit 0 belegt.

demnach wird nur im "freien Sichtfeld" die Distanz gemessen und verwertet.

Ich hänge mal den gesammten Programmcode ran. Falls jemand auch einen SRF02 und einen CMPS03 hat, kann er das ja mal ausprobieren oder modifizieren.

// includes and definitions:

#include "RP6RobotBaseLib.h" 	

#include "RP6I2CmasterTWI.h"

#define CMPS03				0xC0
#define	SRF02				0xE0		
#define attachdistance  	8

uint16_t 	desdirection = 2700;
uint8_t 	obstacle_count = 0;
uint16_t 	area[36] = {0};
// funktions:

uint16_t distance_SRF02(void) //Distanzmessung mit Stopwatches
{
	static uint8_t measureactive = false;		//Messungsstatus
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes
	uint16_t distance;
	
	if (measureactive == false)
	{
		setStopwatch6(0);
		startStopwatch6();
		I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
		measureactive = true;
	}

	if (getStopwatch6() >= 70 && measureactive == true)
	{
		I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
		I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern
		distance = srfbuffer[0] * 256 + srfbuffer[1]-attachdistance; 
		measureactive = false;
		stopStopwatch6();
		
		// hiermit werden Messfehler korregiert, da negative Werte sonst bei anderen funktionen zu Problemen führen			
		if (distance == -attachdistance)			
			distance = 0;			

		 //Anzeige der Distanz auf den LEDs
			statusLEDs.LED1=0;
			statusLEDs.LED2=0;
			statusLEDs.LED3=0;
			statusLEDs.LED4=0;
			statusLEDs.LED5=0;
			statusLEDs.LED6=0;	

			if (((distance % 100)/25 >= 1) || (distance >= 400))		// cm werden auf der einen Seite dargestellt
			statusLEDs.LED1=1;
			if (((distance % 100)/25 >= 2) || (distance >= 400))
			statusLEDs.LED2=1;
			if (((distance % 100)/25 >= 3) || (distance >= 400))
			statusLEDs.LED3=1;											// m auf der anderen
			if (distance / 100 >= 1)
			statusLEDs.LED4=1;
			if (distance / 100 >= 2)
			statusLEDs.LED5=1;
			if (distance / 100 >= 3)
			statusLEDs.LED6=1;

			updateStatusLEDs();
	}
return distance;
}


uint16_t distance_SRF02_blocking(void) //Distanzmessung
{
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes

	I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
	mSleep(65);

	I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern

	return srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
}

uint16_t direction_CMPS03(void) //Richtungsbestimmung
{
	uint8_t cmpsbuffer[2];

	I2CTWI_transmitByte(CMPS03, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); 	// und in dem Array speichern

	return ((cmpsbuffer[0] * 256) + cmpsbuffer[1]); 
}


void allroundscan(void)
{

	uint16_t index = direction_CMPS03() / 100;		// den Index and der aktuellen Richtung starten lassen
	uint16_t counter = 0;
	uint16_t direction = 0;
	uint16_t distance = 0;
	
	writeString_P("\n\tAreascan initiated\n*******************************************\n");
	
	stop();
	changeDirection(RIGHT);
	moveAtSpeed(40,40);


	while(counter < 36)
	{
		task_RP6System();
		direction = direction_CMPS03() / 100;
		
		// Drehrichtung bei übersprungenen Messungen korregieren (funktioniert nicht über den Nullsprung!!!) ausbaufähig
		if ((direction > (index + 1)) && (index > 2))
			changeDirection(LEFT);
		if ((direction < (index -2)) && (index >= 2))
			changeDirection(RIGHT);
		
		// Messwert abspeichern
		if (direction == index)
		{
			distance = distance_SRF02_blocking();
			if (distance >= 1000 || obstacle_left || obstacle_right)	//10m oder ein vom ACS entdecktes Objekt schließt diese Richtung aus.
				area[index] = 0;
			else
				area[index] = distance;
			
			writeString_P("Areascan Value [");
			writeInteger(index*10,DEC);
			writeString_P("]\t=");
			writeInteger(area[index],DEC);
			writeString_P("\n");
			
			changeDirection(RIGHT);
			index++;
			counter++;
		}
		
		// Nullsprung
		if (index >= 36)
			index = 0;
	}
	stop();
	writeString_P("\tAreascan completed\n*******************************************\n");
}

uint16_t direction_calculator(void)
{
	uint16_t maximumsum = 0;
	uint16_t newdirection = 0;
	uint8_t i = 0;
	
	for(i=0;i<36;i++)
	{
		if(i==0)
		{
			if(area[35]+area[i]+area[i+1] > maximumsum)
			{
				maximumsum = area[35]+area[i]+area[i+1];
				newdirection = i;
			}
		}
		else
		{
			if(area[i-1]+area[i]+area[i+1] > maximumsum)
			{
				maximumsum = area[i-1]+area[i]+area[i+1];
				newdirection = i;
			}
		}
		writeString_P("\nMaximum Summe ="); writeInteger(maximumsum,DEC); writeString_P("\t New Direction ="); writeInteger(newdirection*10,DEC);
	}
	return newdirection*100;
}

// movements :

#define	STOP		0
#define	FORWARD		1
#define	LEFT		2
#define RIGHT		3
#define HARDLEFT	4
#define	HARDRIGHT	5
#define TURNLEFT	6
#define	TURNRIGHT	7
#define TURN		8
#define UTURN		9
#define BACKWARD	10
#define OVERRIDE	11

uint8_t movement = STOP;

//cruise

#define	READY		0
#define CRUISING	1
#define CORRECTING	2

uint8_t cruise_state = READY;

//avoid

#define FREE		0
#define AVOID		1
#define ROTATE		2

uint8_t avoid_state = FREE;

//escape

#define	FREE		0
#define OBSTACLE	1
#define BACKWARD	10
#define ROTATE		2

uint8_t escape_state = FREE;

//scan
#define READY		0
#define INIT		1
#define	SCANNING1	2
#define	SCANNING2	3
#define	COMPLETE	4

uint8_t scan_state = READY;


void cruise(void)
{
	if (avoid_state == FREE && escape_state == FREE)
	{
		if (cruise_state == READY)
		{
			cruise_state = CRUISING;
			movement = FORWARD;
			setStopwatch5(0);
			startStopwatch5();
			setLEDs(0b001001);
		}
		
		uint16_t direction = direction_CMPS03();
		
		if ((cruise_state == CRUISING || cruise_state == CORRECTING) && getStopwatch5() >= 2000)
		{
			setLEDs(0b000000);
			
			if (direction > desdirection + 30)
			{
				cruise_state = CORRECTING;
				
				if (direction - desdirection > 3450 && direction - desdirection <= 3600 && (movement != HARDRIGHT || movement != TURNRIGHT))
					movement = RIGHT;
				if (direction - desdirection > 2700 && direction - desdirection <= 3450 && (movement != TURNRIGHT))
					movement = HARDRIGHT;
				if (direction - desdirection > 1800 && direction - desdirection <= 2700)
					movement = TURNRIGHT;
				if (direction - desdirection > 900 && direction - desdirection <= 1800)
					movement = TURNLEFT;
				if (direction - desdirection > 150 && direction - desdirection <= 900 && (movement != TURNLEFT))
					movement = HARDLEFT;
				if (direction - desdirection > 0 && direction - desdirection <= 150 && (movement != HARDLEFT || movement != TURNLEFT))
					movement = LEFT;
			}
			if (desdirection > direction + 30)
			{
				cruise_state = CORRECTING;
				
				if (desdirection - direction > 3450 && desdirection - direction <= 3600 && (movement != HARDLEFT || movement != TURNLEFT))
					movement = LEFT;
				if (desdirection - direction > 2700 && desdirection - direction <= 3450 && movement != TURNLEFT)
					movement = HARDLEFT;
				if (desdirection - direction > 1800 && desdirection - direction <= 2700)
					movement = TURNLEFT;
				if (desdirection - direction > 900 && desdirection - direction <= 1800)
					movement = TURNRIGHT;
				if (desdirection - direction > 150 && desdirection - direction <= 900 && movement != TURNRIGHT)
					movement = HARDRIGHT;
				if (desdirection - direction > 0 && desdirection - direction <= 150 && (movement != HARDRIGHT || movement != TURNRIGHT))
					movement = RIGHT;
			}
		}

		if (cruise_state == CORRECTING)
		{
			if (direction - desdirection >= -30 || direction - desdirection <= 30) // abweichung kleiner 3°
				cruise_state = READY;
		}
	}
}

void avoid(void)
{
	if (escape_state == FREE)
	{
		if (obstacle_left && obstacle_right && avoid_state != ROTATE)
		{
			avoid_state = ROTATE;
			movement = TURN;
			writeString_P("Obstacle Counter= ");
			++obstacle_count;
			writeInteger(obstacle_count,DEC);
			writeString_P("\n");
		}
		else if (obstacle_left)
		{
			avoid_state = AVOID;
			movement = HARDRIGHT;
		}
		else if (obstacle_right)
		{
			avoid_state = AVOID;
			movement = HARDLEFT;
		}
		if (avoid_state == AVOID && (!obstacle_left && !obstacle_right))
		{
			avoid_state = FREE;
			cruise_state = READY;
		}
	}
}

void escape(void)
{
	
	if (escape_state == FREE && (bumper_left || bumper_right))
	{
		stop();
		escape_state = OBSTACLE;
			writeString_P("Obstacle Counter= ");
			++obstacle_count;
			writeInteger(obstacle_count,DEC);
			writeString_P("\n");
	}
	if (escape_state == OBSTACLE)
	{
		if (bumper_left && bumper_right)
		{
			escape_state = BACKWARD;
			movement = TURN;

		}
		else if (bumper_left)
		{
			escape_state = BACKWARD;
			movement = TURNRIGHT;
		}
		else if (bumper_right)
		{
			escape_state = BACKWARD;
			movement = TURNLEFT;
		}
		else
			escape_state = FREE;
	}
}
uint8_t turndirection(void)
{
	uint16_t direction = direction_CMPS03();

	if (direction >= desdirection)
	{
		if (direction - desdirection >= 1800)
			return TURNRIGHT;
		else 
			return TURNLEFT;
	}
	if (desdirection > direction)
	{
		if (desdirection - direction >= 1800)
			return TURNLEFT;
		else
			return TURNRIGHT;
	}
	return STOP;
}


#define CRUISE		100
#define FAST		90
#define MEDIUM		60
#define SLOW		30

void movecontroller(void)
{
	if (obstacle_count >= 3)			// Funktionen zur Richtungsfindung
	{
		stop();
		movement = OVERRIDE;
		allroundscan();
		desdirection = direction_calculator();
		obstacle_count = 0;
		movement = STOP;
		cruise_state = CORRECTING ;
	}
	if (movement != OVERRIDE)			// Ausweichfunktionen
	{
		cruise();
		avoid();
		escape();
		changeDirection(FWD);
	}
	if (movement == TURN)				// Entscheidung welche Drehrichtung vorteilhafter ist
		movement = turndirection();
										// Ausführung der Gewünschten Fahrtbewegung
	if (escape_state == BACKWARD)
	{
		move(MEDIUM, BWD, DIST_MM(100), BLOCKING);
		escape_state = ROTATE;
	}
	if (movement == TURNRIGHT)
	{
		if (escape_state == ROTATE || avoid_state == ROTATE)
		{
			stop();
			rotate(MEDIUM, RIGHT, 90, BLOCKING);
			escape_state = FREE;
			avoid_state = FREE;
			cruise_state = READY;
		}
		else
		{
			changeDirection(RIGHT);
			moveAtSpeed(MEDIUM,MEDIUM);
		}
	}
	if (movement == TURNLEFT)
	{
		if (escape_state == ROTATE || avoid_state == ROTATE)
		{
			stop();
			rotate(MEDIUM, LEFT, 90, BLOCKING);
			escape_state = FREE;
			avoid_state = FREE;
			cruise_state = READY;
		}
		else
		{
			changeDirection(LEFT);
			moveAtSpeed(MEDIUM,MEDIUM);
		}
	}
	
	if (movement == UTURN)
	{
		stop();
		rotate(MEDIUM, RIGHT, 180, BLOCKING);
	}
	if (movement == FORWARD)
		moveAtSpeed(CRUISE,CRUISE);
	if (movement == HARDRIGHT)
		moveAtSpeed(FAST,SLOW);
	if (movement == HARDLEFT)
		moveAtSpeed(SLOW,FAST);
	if (movement == RIGHT)
		moveAtSpeed(FAST,MEDIUM);
	if (movement == LEFT)
		moveAtSpeed(MEDIUM,FAST);
	if (movement == STOP)
		stop();
	if (movement == BACKWARD)
	{
		changeDirection(BWD);
		moveAtSpeed(MEDIUM,MEDIUM);
	}

}


void acsStateChanged(void) // ACS zustand vorne anzeigen
{
	if(obstacle_left && obstacle_right)
		statusLEDs.byte = 0b100100;
	else
		statusLEDs.byte = 0b000000;
	statusLEDs.LED5 = obstacle_left;
	statusLEDs.LED4 = (!obstacle_left);
	statusLEDs.LED2 = obstacle_right;
	statusLEDs.LED1 = (!obstacle_right);
	updateStatusLEDs();
}

/*****************************************************************************/
// Main


int main(void)
{
	initRobotBase(); 
	powerON();
	setLEDs(0b111111);
	mSleep(1000);
	setLEDs(0b001001);
	
	setACSPwrMed();
	
	I2CTWI_initMaster(100);							// I2C speed set to 100kHz
	
	
	ACS_setStateChangedHandler(acsStateChanged);	// Set ACS state changed event handler:
	
	// Main loop
	while(true) 
	{
		task_RP6System();
		movecontroller();
	}
	return 0;
}

Es arbeitet soweit Einwandfrei, aber die Funktion direction_calculator(void) ist noch ausbaufähig.