# Mapping a robot's obstacles based on its position and sensor data

I have a grid in which I'm adding different cell-blocks to, depending on what rotation I'm currently receiving.

• @@@ = Map border
• ### = Obstacle
• 0-9 = Distance to goal-node
• ??? = Goal Path 

Robot's starting posture according to diagram below (ahead-sensors, 0 Rot)

Ahead-sensor --->, Left-sensor ^ and right-sensor v

@@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                         ###                                             @@@
@@@                                     ###  S  ###                                         @@@
@@@                                   4   3 ???   3   4                                     @@@
@@@                               4   3   2 ???   2   3   4                                 @@@
@@@                           4   3   2   1   G   1   2   3   4                              @@@
@@@                               4   3   2   1   2   3   4                                 @@@
@@@                                   4   3   2   3   4                                     @@@
@@@                                       4   3   4                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@                                                                                         @@@
@@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@ @@@


Now i'm currently checking how my robot is positioned so it can add obstacles correctly according to its rotation. i = y, x = j

Right now i'm doing a bunch of if-statements like this (snippet):

(currentRot is between 0-360).

if(currentRot >= 0 && currentRot <= 45){
printf("\n0-45");
if (ir.sensor[5] > FULL_DANGER){ //left
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i-1,newStart.j,-3);printf("\n1,1");goto SUCCESS;} //add obs in map
}
if (ir.sensor[2] > FULL_DANGER){ //right
if(GetCellState(grid,newStart.i+1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i+1,newStart.j,-3);printf("\n1,2");goto SUCCESS;}
}
if (ir.sensor[0] > FULL_DANGER || ir.sensor[7] > FULL_DANGER){ //ahead
if(GetCellState(grid,newStart.i,newStart.j+1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j+1,-3);printf("\n1,3");goto SUCCESS;}
}
}
if(currentRot >= 45 && currentRot <= 90){
printf("\n45-90");
if (ir.sensor[5] > FULL_DANGER){ //left
if(GetCellState(grid,newStart.i,newStart.j-1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j-1,-3);printf("\n1,1");goto SUCCESS;} //add obs in map
}
if (ir.sensor[2] > FULL_DANGER){ //right
if(GetCellState(grid,newStart.i,newStart.j+1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j+1,-3);printf("\n1,2");goto SUCCESS;}
}
if (ir.sensor[0] > FULL_DANGER || ir.sensor[7] > FULL_DANGER){ //ahead
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i-1,newStart.j,-3);printf("\n1,3");goto SUCCESS;}
}
}


Now onto the question: is there a better(smarter) way of checking these conditions and applying the same logic? Like, i thought of using a nested for-loop but exactly how that would work i'm not quite sure of. As of now, it's quite repetitive and ugly.

Entire file can be found here, row 519:

• You can simulate polymorphism in C using function pointers. codereview.stackexchange.com/questions/858/… – Martin Spamer Oct 16 '17 at 11:07
• @MartinSpamer Not quite sure how that would work with my functions. (APK), nor how i would implement it. But i'll read up on it. thanks – Joel Oct 16 '17 at 11:11
• Are you sure you should use both >= and <= when testing the rotation value? Suppose currentRot==45, then both presented if-s will execute, as both conditions are satisfied... – CiaPan Oct 16 '17 at 11:18
• @CiaPan I know what you mean and I've thought about it too, but, that's not really the question. Also, to answer what you wrote: currentRot is never 45.0000, it's always a bunch of decimals, so the chances of it actually being 45.0000 is next to none. What I should do is to actually decrease the span, so. 10-35, 45-80, 100-135 etc... (but 100% accuracy isn't really needed as 1 cell in my grid is 2.5cm. (small irl consequence). – Joel Oct 16 '17 at 11:20
• OK. You haven't shown a compilable piece of code and currentRot remains actually undefined in it, so it was not obvious at all it is of floating-point type... – CiaPan Oct 16 '17 at 11:23

## 2 Answers

Let's start with the code you gave us:

if(currentRot >= 0 && currentRot <= 45){
printf("\n0-45");
if (ir.sensor[5] > FULL_DANGER){ //left
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i-1,newStart.j,-3);printf("\n1,1");goto SUCCESS;} //add obs in map
}
if (ir.sensor[2] > FULL_DANGER){ //right
if(GetCellState(grid,newStart.i+1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i+1,newStart.j,-3);printf("\n1,2");goto SUCCESS;}
}
if (ir.sensor[0] > FULL_DANGER || ir.sensor[7] > FULL_DANGER){ //ahead
if(GetCellState(grid,newStart.i,newStart.j+1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j+1,-3);printf("\n1,3");goto SUCCESS;}
}
}
if(currentRot >= 45 && currentRot <= 90){
printf("\n45-90");
if (ir.sensor[5] > FULL_DANGER){ //left
if(GetCellState(grid,newStart.i,newStart.j-1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j-1,-3);printf("\n1,1");goto SUCCESS;} //add obs in map
}
if (ir.sensor[2] > FULL_DANGER){ //right
if(GetCellState(grid,newStart.i,newStart.j+1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j+1,-3);printf("\n1,2");goto SUCCESS;}
}
if (ir.sensor[0] > FULL_DANGER || ir.sensor[7] > FULL_DANGER){ //ahead
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i-1,newStart.j,-3);printf("\n1,3");goto SUCCESS;}
}
}


I looked at this, and had trouble understanding almost all of it. You've really gone out of your way to chose coding a coding style that's opaque. (This is easy to do in embedded programming for some reason - you have to make a deliberate effort to craft your code in an open, approachable fashion. Please do so.)

Abstraction

For example, your sensors have numbers rather than names. This is just stupid. What are the chances that a robot engineer will add sensors to her robot? 1. A robot engineer is like a teenager with a crappy asian car: they are compelled by some mysterious force to tinker with it! You must assume that your "constants" will change!

#define IR_LEFT   (ir.sensor[5])
#define IR_RIGHT  (ir.sensor[2])
#define IR_FRONT0 (ir.sensor[0])
#define IR_FRONT1 (ir.sensor[7])


Rewriting your code a little bit:

if(currentRot >= 0 && currentRot <= 45){
printf("\n0-45");
if (IR_LEFT > FULL_DANGER){
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i-1,newStart.j,-3);printf("\n1,1");goto SUCCESS;} //add obs in map
}
if (IR_RIGHT > FULL_DANGER){ //right
if(GetCellState(grid,newStart.i+1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i+1,newStart.j,-3);printf("\n1,2");goto SUCCESS;}
}
if (IR_FRONT0 > FULL_DANGER || IR_FRONT1 > FULL_DANGER){
if(GetCellState(grid,newStart.i,newStart.j+1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j+1,-3);printf("\n1,3");goto SUCCESS;}
}
}


Right off the bat, I have the ability to get rid of some of your "explanatory" comments. Getting rid of comments (in favor of "explanatory" code) is always a good thing!

But really, it still looks bad. IR_FRONT1 > FULL_DANGER - WTF does that mean, anyway?

Let's slather on another layer of macro:

#define OBSTACLE_LEFT()  (IR_LEFT > FULL_DANGER)
#define OBSTACLE_RIGHT() (IR_RIGHT > FULL_DANGER)
#define OBSTACLE_AHEAD() (IR_FRONT0 > FULL_DANGER || IR_FRONT1 > FULL_DANGER)


And now:

if(currentRot >= 0 && currentRot <= 45){
printf("\n0-45");
if (OBSTACLE_LEFT()) {
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i-1,newStart.j,-3);printf("\n1,1");goto SUCCESS;} //add obs in map
}
if (OBSTACLE_RIGHT()) {
if(GetCellState(grid,newStart.i+1,newStart.j) != MAP_BORDER){ChangeCellState(grid,newStart.i+1,newStart.j,-3);printf("\n1,2");goto SUCCESS;}
}
if (OBSTACLE_AHEAD()) {
if(GetCellState(grid,newStart.i,newStart.j+1) != MAP_BORDER){ChangeCellState(grid,newStart.i,newStart.j+1,-3);printf("\n1,3");goto SUCCESS;}
}
}


Okay, now I can see what you're checking. Also, of course, if you insist on repeating this a bunch of times, it will be more resistant against typographical errors. (If you mistype ir.sensor[3] instead of [2], the compiler will not complain. But if you mistype OBSTACLE_ROGHT() instead of RIGHT, gcc will pitch a fit.) This is an under-appreciated benefit of abstraction for low-level programming: it's harder for a single typo to compile correctly and cause hours of wasted time debugging.

Style

Now, a couple of words on style. I looked at your full source file, and it's a nightmare. Please keep in mind that when you're asking strangers on the internet (like me) to review your code, you're asking for our time. Also our expertise, but primarily it's time. And you're competing against pictures of cats. And gifs of Kate Upton running on the beach in a bikini.

So you really, really, really need to make sure you don't waste whatever time you get from complete strangers -- who could otherwise be watching a weather forecast from Mexico (GIYF) -- by having a bad coding style!

Please, use a consistent indentation. That means the same amount. Not "2 characters sometimes, and 8 characters other times." Ideally, since I've got a laptop with very high resolution, you'll pick more than 1 or 2 characters of indentation. I rely on indent and outdent to find things like the end of your outer loops and outer if blocks. Of course, I was unable to do that with your code since some of your blocks are at indent level 0, and others seem to change indentation levels at random.

Next, pick a consistent spacing rule. Look at the "example" code you gave us:

if(currentRot
if (ir.sensor[5]


Why does one if have a space after it, while the next one does not? Anyway, dropping those spaces doesn't make your code "look cool." Neither does setting your terminal to black with a green foreground, or whatever. It makes your code look crowded, and hard to read, and did I mention that I could be watching the weather forecast in Mexico? Maxima! Minima! Noche!

There are hundreds of "coding style" guidelines on the interwebs. Your class might have one. Your school might have one. If not, google for one. Pick the first one you find, and follow it - it doesn't matter how stupid it is, it's bound to be better than your existing code because it will be consistent. Even consistent + stupid is better than random.

If you're lucky, your text editor might even have a code-beautifier. If not, look for one. Try "code beautifier" or "indent" for keywords.

Stop Repeating Yourself

I'm a little surprised to have to write this, but you've got essentially the same code appearing over and over and over again in your if statements:

if (OBSTACLE_LEFT()) {
if(GetCellState(grid,newStart.i-1,newStart.j) != MAP_BORDER){
ChangeCellState(grid,newStart.i-1,newStart.j,-3);
printf("\n1,1");
goto SUCCESS;
} //add obs in map
}
if (OBSTACLE_RIGHT()) {
if(GetCellState(grid,newStart.i+1,newStart.j) != MAP_BORDER){
ChangeCellState(grid,newStart.i+1,newStart.j,-3);
printf("\n1,2");
goto SUCCESS;
}
}


Here's a link to a useful blog post.

So let's see... if the cell state at some location is not the map border, then change the cell state to -3 and print some kind of debug output and goto SUCCESS. (Seriously? What is -3? Did you mean OBSTACLE?)

I'm going to ignore the fact that you're skipping over a check for another obstacle, because I can't read your code. It might make sense to do that.

But each of those blocks - which are so long, and so "common" that you deliberately folded them up onto a single line - is really the same code over and over again. And in the 1970's they had a mechanism for that: functions.

So why not write a function to handle all that code for you?

if (OBSTACLE_LEFT()) {
updateMap(LEFTWARDS_OF(newStart), OBSTACLE);
goto SUCCESS; // ?really?
}
if (OBSTACLE_RIGHT()) {
updateMap(RIGHTWARDS_OF(newStart), OBSTACLE);
goto SUCCESS; // ?really?
}


Or you could call it "report_obstacle" and get rid of one more parameter.

Encapsulation

And now we come to the "real" answer to your question. Because, if we try rewriting your original code to look like the code I just proposed, what happens? I'll skip the goto statements, since I think they're a mistake, and since they don't make a difference to this:

if(currentRot >= 0 && currentRot <= 45){
printf("\n0-45");

if (OBSTACLE_LEFT()) {
report_obstacle(newStart.i - 1, newStart.j);
}
if (OBSTACLE_RIGHT()) {
report_obstacle(newStart.i + 1, newStart.j);
}
if (OBSTACLE_AHEAD()) {
report_obstacle(newStart.i, newStart.j + 1);
}
}
if(currentRot >= 45 && currentRot <= 90){
printf("\n45-90");

if (OBSTACLE_LEFT()) {
report_obstacle(newStart.i, newStart.j - 1);
}
if (OBSTACLE_RIGHT()) {
report_obstacle(newStart.i, newStart.j + 1);
}
if (OBSTACLE_AHEAD()) {
report_obstacle(newStart.i - 1, newStart.j);
}
}


Apparently, your concern with currentRot really translates into a change in the values you pass to report_obstacle (or whatever you want to call it).

BUT WAIT!

Because I've looked at your code, I know that currentRot is really a translation of now.th - the theta value of your robot's current posture. Just exactly as newStart is a translation of two other elements of your robot's current posture!

So why not just define a function that encodes these rules for you? Then you won't have to drag them up into this higher level code! Just write a group of functions that convert "relative" references into map references:

Position left_of_robot();      // Return x,y left of robot
Position right_of_robot();     // Return x,y right of robot
Position ahead_of_robot();     // Return x,y ahead of robot
Position position_of_robot();  // Return robot.x and .y


Then you can reduce your code by 50%:

printf("\nI don't care where the robot is facing because my code just works!");

if (OBSTACLE_LEFT()) {
report_obstacle(left_of_robot());
}
if (OBSTACLE_RIGHT()) {
report_obstacle(right_of_robot());
}
if (OBSTACLE_AHEAD()) {
report_obstacle(ahead_of_robot());
}


I think this code is much clearer, and easier to understand. I think all the code is at the same level of abstraction here. So if someone adds a change, it should stay at this level of abstraction, which makes maintenance smoother.

• For some reason i never saw this answer, but its pure gold, like, i laughed like crazy. One thing i didnt laugh about was this terrible piece of code. Looking back, Im ashamed that i even posted it. Thanks for your time and effort Mr forecast. Maybe you’ll get a giggle or two out of reading it again. – Joel Sep 11 '18 at 14:19
• It's good to be appreciated, even if it takes a while. It's like being a parent... and what are you doing all the time in your room with the door closed? – Austin Hastings Sep 15 '18 at 14:19

If you extract out the common code that gets executed it's

if(GetCellState(grid,newStart.i+offI,newStart.j+offJ) != MAP_BORDER){
ChangeCellState(grid,newStart.i+offI,newStart.j+offJ, -3);printf("\n1,3");goto SUCCESS;
}
`

In other words you can just use the angle to select the offsets you need to check and then check the board based on the offsets.