Both X and Y position only go up?

```
else {
LocalRobotPosX = (2 * sin(ChangeAngle / 2)) * (WheelTravelX2 / ChangeAngle);
LocalRobotPosY = (2 * sin(ChangeAngle / 2)) * (WheelTravelY2 / ChangeAngle);
}
```

Looking at this, I’m not certain how this is calculating X and Y position. ‘ChangeAngle’ is updating once every 10 milliseconds (pretty quickly), so it will be fairly small.

And looking at the ‘if’ statement that comes before this:

```
if (ChangeAngle < .3 && ChangeAngle > -.3) {
LocalRobotPosX = WheelTravelX2;
LocalRobotPosY = WheelTravelY2;
}
```

The loop might run this section instead of the second part since ‘ChangeAngle’ is small, regardless of the robot’s angle. So your X position and Y position would just equal the wheel rotation, not field position.

The main idea when calculating coordinates is increasing/decreasing the impact each wheel has on X and Y position by using sin/cos. You want to **multiply the change in wheel rotation by the sin/cos of the robot’s current angle**. The wheel rotation is ‘how far I’ve gone’ and the sin(Angle) or cos(Angle) is ‘how much that distance affects X or Y position.’

If you’re driving forwards at 0°, only Y position should increase.

You find the change in rotation…

*Rotation - LastRotation = RotationChange*

…and find how much to multiply that value by

*cos(0 • π/180) = sin(0) = 1*

The Y position gained from this would be *Y = 1 * RotationChange*, as this is a straight line.

When the angle is different, 45° for example, Y position would still increase, but at a lesser amount.

*Y = cos(45 • π/180) • RotationChange, Y = 0.707 • RotationChange*

As the Angle increases, cos(Angle) decreases. If the robot is facing 90° (moving along the X-axis), *cos(90) = 0*, meaning that RotationChange has no effect on the Y position.

This is how you calculate Y position. To calculate X position, simply swap the cos(Angle) to sin(Angle). *sin(90) = 1*, So when moving along the X-axis, X position will increase.

*PositionY = cos(Angle • π/180) • RotationChange*

PositionX = sin(Angle • π/180) • RotationChange

```
PositionY += cos(Angle * 3.1415926536 / 180) * (Tracker.position(deg) - LastPosition);
PositionX += sin(Angle * 3.1415926536 / 180) * (Tracker.position(deg) - LastPosition);
```

Coordinate calculation for two tracking wheels would look something like this:

```
while(1) {
PositionX += cos(Angle * 3.1415926536 / 180) * (TrackerX.position(deg) - LastPositionX) - sin(Angle * 3.1415926536 / 180) * (TrackerY.position(deg) - LastPositionY);
PositionY += sin(Angle * 3.1415926536 / 180) * (TrackerX.position(deg) - LastPositionX) - cos(Angle * 3.1415926536 / 180) * (TrackerY.position(deg) - LastPositionY);
LastPositionX = TrackerX.position(deg);
LastPositionY = TrackerY.position(deg);
Angle = Gyro.rotation(deg);
}
```

This is our code (we just added a second tracking wheel today, but the concept remains the same). Make sure to use ‘PositionX/Y +=’ instead of ‘=’ so it adds up each little increment in position, otherwise it will give 0.

That concludes this TED talk. Hope this helps.