예제 #1
0
/*
 * set drive parameters from driveParameterBuffer if new parameters are available,
 * otherwise stop the car when specified duration expires for security reasons
 */
static void driveTask(void) {
	portTickType duration_ms = 0, directionPercent = 0, currentTime = 0,
			lastWakeTime = 0, startTime = 0;
	driveParameters par;
	int8_t queueReturnValue, speed = 0;
	double Ergebnis[3];
	gps_reducedData_t *own;

	lastWakeTime = os_getTime();

	for (;;) {
//		calls[6]++;
#ifdef SLAM_SCENARIO
		os_frequency(&lastWakeTime, 1);
#else
		os_frequency(&lastWakeTime, 60);
#endif
		currentTime = os_getTime();

		//get new drive commands from queue
		queueReturnValue = xQueueReceive(driveParameterBuffer, &par, 10);
		if (queueReturnValue == pdTRUE) {
			directionPercent = par.direction;
			speed = par.speed;
			startTime = par.startTime;
			duration_ms = par.duration;
		}
		if (hall != start){
			start = !start;
			if (os_getTime() - driveTime > 400){
				distance++;
				#ifdef SLAM_SCENARIO
				Drive_SetMotor(0);
				os_wait(200);
				own = get_ownCoords();
				SLAM_Algorithm((int32_t)CoordinatesToMap((int32_t)own->x),(int32_t)CoordinatesToMap((int32_t)own->y),(int32_t)uint16DegreeRelativeToX(own->angle),get_accumulateSteeringAngle()/MAX_STEERING_ANGLE, (int32_t) HALL_RESOLUTION, Ergebnis);
//				  send_Element(22);
//				  send_Element((int16_t)Ergebnis[0]);
//				  send_Element((int16_t)Ergebnis[1]);
//				  send_Element(-1);
				set_ownCoords(MapToCoordinates((int16_t)Ergebnis[0]),MapToCoordinates((int16_t)Ergebnis[1]),MapToCoordinates((int16_t)Ergebnis[2]));
				accumulateSteeringAngle(INT_LEAST8_MAX);
				#endif
			}
		}

		//set speed and direction if specified duration not expired, else stop the car
		if (duration_ms < 600){
			if (currentTime - startTime < duration_ms) {
			#ifdef DEBUG2
				printf("\nDRIVE\nset motor speed to %d and angle to %d\n\r", speed,
						directionPercent);
			#endif
				Drive_SetServo(directionPercent);
				Drive_SetMotor(speed);
				driveDistance();
			} else {
				Drive_SetMotor(0); //stop if time period expired;
				distance = 0;
				realdistance = 0;
			}
		}
		else{
			Drive_SetServo(directionPercent);
			Drive_SetMotor(speed);
			if (speed == 0){
				distance = 0;
				realdistance = 0;
			}
			driveDistance();
		}
	}
}
예제 #2
0
파일: api.c 프로젝트: el303/pses
/**
 * SensorRange = einschränkung der Reichweite. Verhindert unnötig lange Berechnung
 * Return: Distance
 */
uint16_t driven_before(int8_t type, uint16_t SensorRange) {
	double diffR;
#ifdef CARTOGRAPHY_SCENARIO
	gps_reducedData_t *own;
	uint16_t angle, diffL;
	int16_t xCollision = 0, yCollision = 0, ownX, ownY;
	int32_t newX, newY;

	//Aktuelle Orientierung beachten!!! Orientierung in Abhängigkeit der Fahrzeugorientierung und dem aktuell betrachteten Sensor
	own = get_ownCoords();
	switch(type) {
		case RIGHT_SENSOR:
		angle = AngleMinus(own->angle, 16383);
		if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2; //siehe Anforderungen in wallFollow für align = 2
		break;
		case LEFT_SENSOR:
		angle = AnglePlus(own->angle, 16383);
		if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;//siehe Anforderungen in wallFollow für align = 2
		break;
		case FRONT_SENSOR:
		angle = own->angle;
		if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;
		break;
		default: angle = own->angle; break;
	}
	SensorRange = SensorRange / 5;

	ownX = CoordinatesToMap(own->x);
	ownY = CoordinatesToMap(own->y);

	if (Sema != NULL) {
		while ( xSemaphoreTake( Sema, ( portTickType ) 10 ) != pdTRUE ) {
			os_wait(10);
		}
		newX = (SensorRange*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX;
		ownX = (2*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX; //Ein stück aus dem Urpsrung rausgehen
		newY = (SensorRange*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
		ownY = (2*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;

		//gibt es ein bereits befahrenes Gebiet/Hindernis das zwischen Sensorwert und Auto liegt?
		diffL = calc_reachability(&xCollision, &yCollision, ownX, ownY, (int16_t)newX, (int16_t)newY);

		xSemaphoreGive(Sema);
	} else return 600; //600 = max. Sensorreichweite

	if (diffL == 1) {
		diffR = sqrt(abs16(ownX - xCollision)*abs16(ownX - xCollision) + abs16(ownY - yCollision)*abs16(ownY - yCollision));
		diffR = diffR*5;
		if (diffR < 100) {
			Seg_Hex(0x00);
		}
		else Seg_Hex(0x01);

	}
	else {
		diffR = 600; // 600 für nichts gefunden
	}
	if (diffR > 600) diffR = 600;
#endif
	return (uint16_t) diffR;
}