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;
}