#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, linksrumvoid 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. *********************************************************************/