/*
* 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();
}
}
}
/**
* 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;
}
