/**
* Überprüft auf eine Kollision
* Return: diff = neues Struct direkt zum Ziel
* return: 0 wenn keine Kollision, 1 sonst
*/
uint8_t calc_Offroad(gps_reducedData_t* diff, gps_reducedData_t* own, gps_reducedData_t* target){
int16_t xCollision, yCollision;
tree_init(CoordinatesToMap(own->x), CoordinatesToMap(own->y));
if (calc_reachability(&xCollision, &yCollision,CoordinatesToMap(own->x), CoordinatesToMap(own->y),CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){
setCoordinates(diff, target->cam_id, target->tag_id, target->x, target->y, target->angle, target->isWorld);
return 0;
}
else{
scan_Obstacles(xCollision, yCollision, own, target, CoordinatesToMap(own->x), CoordinatesToMap(own->y)); //übergibt hier die Kollisionskoordinaten
}
return 1;
}
/*
* 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();
}
}
}
/**
* Findet die 2 (Rand)Koordinaten die das Hindernis aus Sicht des Autos abgrenzen. Findet von dort einen Weg zum Ziel
*/
void scan_Obstacles(int8_t xCollision, int8_t yCollision, gps_reducedData_t* own, gps_reducedData_t* target, int8_t x_old, int8_t y_old){
int16_t x_freeL, y_freeL, x_freeR, y_freeR;
int8_t xleft = -1, yleft = -1, xright = -1, yright = -1;
//positiver Pfad
//Annäherung an Hindernis von unten
if(get_coord_obstacle(xCollision, yCollision-1) == 0 && (get_coord_obstacle(xCollision-1, yCollision) == 1 || get_coord_obstacle(xCollision+1, yCollision) == 1)){
move_AroundHorizontal( &xleft, &xright, &yleft, &yright, xCollision, yCollision, 1);
if (xleft >= 0 && yleft >= 0){
if (calc_reachability(&x_freeL, &y_freeL,CoordinatesToMap(own->x), CoordinatesToMap(own->y), xleft, yleft) == 1){
x_freeL = xCollision;
y_freeL = yCollision; //von Hinderniskante zu Auto ist kein direkter Weg möglich. Nehme zwischeschritt
}
}
if (xright >= 0 && yright >= 0){
if (calc_reachability(&x_freeR, &y_freeR, CoordinatesToMap(own->x), CoordinatesToMap(own->y), xright, yright) == 1){
x_freeR = xCollision;
y_freeR = yCollision;
}
}
}
else
//Annäherung an Hindernis von oben
if(get_coord_obstacle(xCollision, yCollision-1) != 0 && get_coord_obstacle(xCollision,yCollision+1) == 0 && (get_coord_obstacle(xCollision-1, yCollision) == 1 || get_coord_obstacle(xCollision+1, yCollision) == 1)){
move_AroundHorizontal( &xleft, &xright, &yleft, &yright, xCollision, yCollision, -1); // -1 = Hindernis von oben
if (xleft >= 0 && yleft >= 0){
if (calc_reachability(&x_freeL, &y_freeL,CoordinatesToMap(own->x), CoordinatesToMap(own->y), xleft, yleft) == 1){
x_freeL = xCollision;
y_freeL = yCollision; //von Hinderniskante zu Auto ist kein direkter Weg möglich. Nehme zwischeschritt
}
}
if (xright >= 0 && yright >= 0){
if (calc_reachability(&x_freeR, &y_freeR, CoordinatesToMap(own->x), CoordinatesToMap(own->y), xright, yright) == 1){
x_freeR = xCollision;
y_freeR = yCollision;
}
}
}
else
//Sonderfall senkrechtes Hindernis
if((get_coord_obstacle(xCollision, yCollision-1) != 0 && get_coord_obstacle(xCollision,yCollision+1) == 1) || isEcke(xCollision,yCollision) == 1 ){
//Überpürung ob Weg Rechts oder Links vom Hinderniss
if (get_coord_obstacle(xCollision-1, yCollision) == 0 || get_coord_obstacle(xCollision-1, yCollision-1) == 0 || get_coord_obstacle(xCollision-1, yCollision +1) == 0){
move_AroundVertical( &xleft, &xright, &yleft, &yright, xCollision, yCollision, 1); //Links
x_freeL = 1;
}
else if (get_coord_obstacle(xCollision+1, yCollision) == 0 || get_coord_obstacle(xCollision+1, yCollision-1) == 0 || get_coord_obstacle(xCollision+1, yCollision +1) == 0)
move_AroundVertical( &xleft, &xright, &yleft, &yright, xCollision, yCollision, -1);
if (xleft >= 0 && yleft >= 0){
if (calc_reachability(&x_freeL, &y_freeL,CoordinatesToMap(own->x), CoordinatesToMap(own->y), xleft, yleft) == 1){
x_freeL = xCollision;
y_freeL = yCollision;
}
}
if (xright >= 0 && yright >= 0){
if (calc_reachability(&x_freeR, &y_freeR, CoordinatesToMap(own->x), CoordinatesToMap(own->y), xright, yright) == 1){
x_freeR = xCollision;
y_freeR = yCollision;
}
}
}
//hier neue Zwischenpunkte abspeichern
if(xleft >= 0 && yleft >= 0 ){
if(x_freeL == xCollision){
tree_insert(x_freeL, y_freeL, x_old, y_old, 1); //zwischenschritt vom zwischenschritt abspeichern
tree_insert(xleft,yleft,x_freeL,y_freeL, 1);
}
else tree_insert(xleft,yleft,x_old,y_old,1); //ansonsten nur die neuen Koordinaten abspeichern
//Prüfen ob Ziel bereits direkt erreichbar
if (calc_reachability(&x_freeL, &y_freeL, xleft, yleft, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){ // In diesem Fall ist das Ziel erreicht
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xleft,yleft,1);
return;
}
//Hier berechnen wie Dick das Hinderniss ist und gleich neuen Wegpunkt abspeichern -> reduziert Anzahl rekursiver Aufrufe
if(find_Pos_Depth(&x_freeL, &y_freeL, xleft, yleft) == 1){
tree_insert(x_freeL,y_freeL,xleft,yleft,1);
xleft = x_freeL;
yleft = y_freeL;
}
if (calc_reachability(&x_freeL, &y_freeL, xleft, yleft, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){ // In diesem Fall ist das Ziel erreicht
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xleft,yleft,1);
}
else{
setCoordinates(own, own->cam_id,own->tag_id,MapToCoordinates(xleft),MapToCoordinates(yleft),own->angle,own->isWorld);
scan_Obstacles(x_freeL, y_freeL, own, target, xleft, yleft);
}
}
if(xright >= 0 && yright >= 0){
if(x_freeR == xCollision){
tree_insert(x_freeR, y_freeR, x_old, y_old, 0); //zwischenschritt vom zwischenschritt abspeichern
tree_insert(xright,yright,x_freeR,y_freeR, 0);
}
else tree_insert(xright,yright,x_old,y_old,0);
if (calc_reachability(&x_freeR, &y_freeR, xright, yright, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xright,yright,0);
return;
}
if(find_Pos_Depth(&x_freeR, &y_freeR, xright, yright) == 1){
tree_insert(x_freeR,y_freeR,xright,yright,0);
xright = x_freeR;
yright = y_freeR;
}
if (calc_reachability(&x_freeR, &y_freeR, xright, yright, CoordinatesToMap(target->x), CoordinatesToMap(target->y)) == 0){
tree_insert(CoordinatesToMap(target->x), CoordinatesToMap(target->y), xright,yright,0);
}
else{
setCoordinates(own, own->cam_id,own->tag_id,MapToCoordinates(xright),MapToCoordinates(yright),own->angle,own->isWorld);
scan_Obstacles(x_freeR, y_freeR, own, target, xright, yright);
}
}
return; //toter Pfad, wird nicht mehr zur Zielfindung benötigt
}
/**
* 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;
}