[BUG] Alg broken

[jazonshou]

Describe the bug The output calculation is broken. It seems to only account for rotation and moving forward, and does not move left/right.

To Reproduce Steps to reproduce the behavior:

1. Run the code in main.cpp

Expected behavior Should go to 2_ft, 2_ft, 45_deg (forward 2 ft, right 2 ft, turn 45 deg)

Actual behavior Goes to 2_ft, 0_ft, 45_deg (forward 2 ft, right 0 ft, turn 45 deg)

Screenshots n/a

Additional context n/a

[jazonshou]

bruh