Best way to find the shortest rotational path between current and desired rotation

Question

I'm trying to build a sort of 3 dimensional robotic arm that mimics the movement of a virtual robotic arm inside Unreal Engine. I'm using Arduino Mega for the hardware side of the arm.

I have a MPU6050 gyroscope on the robotic arm and I wrote a simple code to mimic the virtual arm's rotation in the real world.

For example, when the virtual arm's yaw angle is at 0, the hardware arm's angle is also 0, which is set by the MPU6050 gyro.

So when the virtual arm moves 90 degrees left, the hardware arm rotates left till the MPU6050 gives a reading of around 90 degrees then stops.

The problem is, Unreal Engine forwards the arm's rotational value in a range of -180 to 180 degrees and the hardware is hard-coded to either go left if the desired rotation is below the current rotation. Aka if the desired rotation is -90 degrees, and current rotation is 0, for left and vice versa, the robotic arm goes right for a +90 desired rotation. This becomes an issue as I reach the end limits till -179 or 179 as the virtual arm will go to -178, -179 then +179, +178, +177 and so on and in order to replicate that desired rotation, the hardware arm would turn left till -179, then start spinning right until it reaches +179.

I'd like to implement some sort of shortest path algorithm that would take the distance between the current and desired rotation and choose whether to go left or right based on which part is the shortest.

Help appreciated. I hope I was able to clarify what exactly it is I'm trying to achieve.

Answer 1

So after posting this question, I had a brain-fart and came up with the solution myself.

The solution is to take the current and desired rotation, and then loop twice from the current to desired rotation. The first time, you increment the loop and second time you decrement the loop. Inside the loops, you increment a "cost" variable for each iteration. You also add in an if statement in the increment loop with the condition that if i >= 179, set i to -179, and vice versa for the decrement i.e: if i <= -179, set i to 179. This will ensure the loops stay between the range -179 to 179 at all times. Then just compare both of the cost variables to see which cost is less and if the decrement loop has lower cost, you go left, and if the increment loop has lower cost, you go right.

Pasted below, is the entire arduino code for this. Upload to your board, set Serial baud rate according to your board and enter a desired rotational value in the serial monitor and the code should show you cost of both rotational loops as well as the final decision.

void setup() { // put your setup code here, to run once: Serial.begin(2000000); Serial.setTimeout(5); }

int CurrentRot,DesiredRot;

void loop() { String Command; Command = Serial.readString();

if (Command != "") //Cut Command String into Substrings { DesiredRot = Command.toFloat();

String Result;
Result = FindShortestPath(CurrentRot,DesiredRot);
Serial.println(Result);

}

}

String FindShortestPath(int CurrentRotation, int DesiredRotation) {

Serial.print("CurrentRot: "); Serial.println(CurrentRotation); Serial.print("DesiredRot: "); Serial.println(DesiredRotation);

int CostLeft=0,CostRight=0,iLeft=0,iRight=0; String Decision;

iLeft = CurrentRotation ; while( iLeft != DesiredRotation) // Check Cost going left per iteration { iLeft++; Serial.print("iLeft: "); Serial.println(iLeft);

delay(10);
CostLeft++;                                             // Add cost per iteration
if(iLeft >= 179)                                           // If reached rotational limit of 179
{
  iLeft = -179;                                            // Jump to -179 and continue going left
}

}

iRight = CurrentRotation ; while( iRight != DesiredRotation) // Check Cost going right per iteration { iRight--; Serial.print("iRight: "); Serial.println(iRight); delay(10); CostRight++; // Add cost per iteration if(iRight <= -179) // If reached rotational limit of -179 { iRight = 179; // Jump to +179 and continue going right } }

Serial.print("CostLeft: "); Serial.println(CostLeft); Serial.print("CostRight: "); Serial.println(CostRight);

if(CostLeft < CostRight) // if left iteration cost is less than right, return left { Decision = "left"; return Decision; } else if(CostRight < CostLeft) // if right iteration cost is less than left, return right { Decision = "right"; return Decision; }

}