# Calculate X,Y Coordinates with Distance and Angle Sensor Data for iRobot Create Command Module

### C

#### Public Domain

taken from [http://groups.google.com/group/irobot-create-forum/web/calculate-x-y-coordinates-with-distance-and-angle-sensor-data]

Download (right click, save as, rename as appropriate)

#### Embed

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38``` ```/******************************** currentX, currentY - uint16_t globals to maintain current coordinates targetX, targetY - uint16_t globals hold the coordinates we want to head toward targetAngle - uint16_t global holds the direction we should head (line-ofsite) to reach targetX,targetY targetDistance - uint16_t global holds the distance (line-of-site) to targetX,targetY ********************************/ void CalcNewPosition(int16_t dist, int16_t angl) { // calculate new position using dist and angl double x,y,h; // update current x,y coordinats // radians = angl/57.296 // newx = dist * sin(radians) // newy = dist * cos(radians) currentX += ( dist * sin( angl / 57.296 )); currentY += ( dist * cos( angl / 57.296 )); // calculate heading to targetX,targetY from currentX,currentY // (solve: tan t = opposite/adjacent // == atan2(adjacent,opposite) - convert 0..3.14159 radians to to 0..359 degrees targetAngle=(atan2(currentX-targetX, currentY-targetY)*1000) / 17.4533; targetAngle+=180; if(targetAngle>359) targetAngle-=360; // calculate distance directly to target // (solve for the hypotenuse of the triangle) hypotenuse=sqrt(x^2 + y^2) if(currentX>targetX) x=currentX-targetX; else x=targetX-currentX; if(currentY>targetY) y=currentY-targetY; else y=targetY-currentY; h=sqrt(x*x+y*y); targetDistance=h; } ```