targetdrive.cpp

一个简单使用的控制器

#include "TargetDrive.h"
#include <math.h>

TargetDrive::TargetDrive(int tX, int tZ) {
	targetX = tX;
	targetZ = tZ;
	robotAction = new RobotAction();
	vfhState = new VFHState();
	freeSectorsThreshold = 5;
	printf("TargetDrive created\n");
}

double TargetDrive::driveTheta(double dTh) {
	if(dTh > 25)
		return 25;
	if(dTh < -25)
		return -25;
	return dTh/2;
}

double TargetDrive::driveVel(double distSquare) {
	if(distSquare > 100*100)
		return 100;
	else
		return sqrt(distSquare);
}

// TODO: Kreisschluss
// liefert aus numSectors Sektoren in freeSpaces angefangen von startSect an alle Anfangsindizes der Stuecke von Groesse mind pieceSize, linksrum
void TargetDrive::freePieces(int freeSpaces [], int numSectors, int targetSect, int pieceSize, int** pieces, int piecesSize) {
	int sectCount = 0;
	int sectStart = -1;
	int sectSize = 0;
	int curSect = targetSect;

	*pieces = new int[2 * numSectors];		// das sollte reichen, oder ?
	piecesSize = 0;
	
	for(sectCount = 0; sectCount < numSectors; sectCount++) {
		curSect = targetSect + sectCount;
		curSect %= numSectors;
		if(freeSpaces[curSect] == 0) {
			if(sectStart == -1)
				sectStart = curSect;

			sectSize++;		
		} else {
			if(sectSize >= pieceSize) {
				printf("Found Sector: Start: %d, Size: %d\n", sectStart, sectSize);
				(*pieces)[piecesSize] = sectStart;
				piecesSize++;
				(*pieces)[piecesSize] = sectSize;
				piecesSize++;
			}
			sectStart = -1;
			sectSize = 0;
		}
	}

	if(freeSpaces[curSect] == 0) {		// das letzte Stueck ist noch "offen"
		int count;	// wieder von vorne anfangen
		for(count = 0; count < numSectors; count++) {
			curSect = targetSect + count;
			curSect %= numSectors;
			if(freeSpaces[curSect] == 0) {
				sectSize++;
			} else {	// ende des ueberlappenden sektors
				if(sectSize >= pieceSize) {
					printf("Found Sector: Start: %d, Size: %d\n", sectStart, sectSize);
					(*pieces)[piecesSize] = sectStart;
					piecesSize++;
					(*pieces)[piecesSize] = sectSize;
					piecesSize++;
				}
				break;	// nur einen Sektor verfolgen
			}
		}
	}

	return;
}



// TODO: direkt um targetsect

/**
 *	berechnet den ersten Sektor vom ersten Vorkommen von num freien Sektoren nacheinander links von targetTh (inkl.)
 *	targetTh - Winkel th im Raum
 *	theta - Roboterwinkel im Raum
 */
int TargetDrive::firstFreeSectorsLeft(double targetTh, int threshold, double theta) {
   threshold=threshold;
	// Sektor von targetTh bestimmen.
	double sectorTheta = targetTh - theta;		// globale Koordinate -> lokale K.
	
	while(sectorTheta > 360)
		sectorTheta -= 360;
	while(sectorTheta < - 360)
		sectorTheta += 360;

	features_vfh_pieces_message vfh = vfhState->getVFH();
	int numSectors = vfh.numSectors;
	double sectSize = 360/(double)numSectors;
	int targetSect = (int) (sectorTheta / sectSize);
	targetSect %= numSectors;

	printf("targetTh: %.2f, robotTheta: %.2f, TargetSector: %.2f deg, %d \n", targetTh, theta, sectorTheta, targetSect);

//	printf("\n\n1");
//	freePieces(vfh.freespaceValues, numSectors, targetSect, 1);
	printf("\n\n5");
	int* pieces = NULL;
	int piecesSize;
	freePieces(vfh.freespaceValues, numSectors, targetSect, freeSectorsThreshold, &pieces, piecesSize);

	if(piecesSize == 0)		// nichts frei
		return -1;

	int firstSectStart = pieces[0];
	//int firstSectSize = pieces[1];
	
	return firstSectStart;
}


/**
 *	berechnet den ersten Sektor vom ersten Vorkommen von num freien Sektoren nacheinander rechts von targetTh (inkl.)
 *	targetTh - Winkel th im Raum
 *	theta - Roboterwinkel im Raum
 */
int TargetDrive::firstFreeSectorsRight(double targetTh, int threshold, double theta) {
   threshold=threshold;
	// Sektor von targetTh bestimmen.
	double sectorTheta = targetTh - theta;		// globale Koordinate -> lokale K.
	
	while(sectorTheta > 360)
		sectorTheta -= 360;
	while(sectorTheta < - 360)
		sectorTheta += 360;

	features_vfh_pieces_message vfh = vfhState->getVFH();
	int numSectors = vfh.numSectors;
	double sectSize = 360/(double)numSectors;
	int targetSect = (int) (sectorTheta / sectSize);
	targetSect %= numSectors;

	printf("targetTh: %.2f, robotTheta: %.2f, TargetSector: %.2f deg, %d \n", targetTh, theta, sectorTheta, targetSect);

//	printf("\n\n1");
//	freePieces(vfh.freespaceValues, numSectors, targetSect, 1);
	printf("\n\n5");
	int* pieces = NULL;
	int piecesSize;
	freePieces(vfh.freespaceValues, numSectors, targetSect, freeSectorsThreshold, &pieces, piecesSize);

	if(piecesSize == 0)		// nichts frei
		return -1;

	int lastSectStart = pieces[piecesSize - 2];
	int lastSectSize = pieces[piecesSize - 1];
	
	int startRight = lastSectStart + lastSectSize;
	if(startRight > numSectors)
		return 0;	// Weg direkt ist frei

	return startRight;
}

double TargetDrive::getSafeTheta(double targetTh, double theta) {
	if(!vfhState->init)
		return 0;
	
	printf("num right 5\n");
	int sectRight = firstFreeSectorsRight(targetTh, freeSectorsThreshold, theta);
	int sectLeft = firstFreeSectorsLeft(targetTh, freeSectorsThreshold, theta);
printf("SafeTheta: right %d left %d\n", sectRight, sectLeft);
	if((sectRight == 0) || (sectLeft == 0))
		return targetTh;

	int goalSect = -1;
	if(sectRight == -1)
		goalSect = sectLeft;
	if(sectLeft == -1)
		goalSect = sectRight;

	features_vfh_pieces_message vfh = vfhState->getVFH();
	if(sectLeft < (vfh.numSectors - sectRight))
		goalSect = sectLeft;
	else
		goalSect = sectRight;

	double ret = goalSect * 360.0/((double)vfh.numSectors);
printf("GoalTheta: %.2f\n", ret);
	return ret;
}

void TargetDrive::driveFrom(int x, int z, int theta) {
//	printf("Start driveFrom\n");
	int dx = targetX - x;
	int dz = targetZ - z;
	double targetTh = ((180.0/M_PI*atan2((double)dx, (double)dz)));
	targetTh += 180;
	double goalTh = getSafeTheta(targetTh, theta);
	goalTh += 180;
	double dTh = goalTh - theta;
	while(dTh > 180)
		dTh -= 360;
	while(dTh < -180)
		dTh += 360;
	
	printf("Drive: dx:%d dz:%d targetTh:%.2f, dTh: %.2f\n", dx, dz, targetTh , dTh);
	
	double distSquare = dx*dx + dz*dz;
	
 	if((dTh > -10) && (dTh < 10)) {
		double vDrive = driveVel(distSquare);
		robotAction->setVelocity((int)vDrive,0);
		printf("Driving forward: %d\n", (int)vDrive);
	} else {
		double thDrive = driveTheta(dTh);
		printf("Turning around: %d\n", (int)thDrive);
		robotAction->setVelocity(0, (int)thDrive);
	} 
}



/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/