Simple: You have to return two values: an angle and a motor index. Your code also has a bug, max_angle isn't initialized. For two fixed values its an overkill to use a for loop. Using a for loop would be reasonable only if the input angles were in an array. Passing in the motor indices is also a waste, we know that they can only be zero or 1, they are constants.
void Poly_servo_goto(float angle1, float angle2, uint8* result_index, float* result_angle)
{
if (angle1 >= angle2)
{
*result_index = 0;
*result_angle = angle1;
}
else
{
*result_index = 1;
*result_angle = angle2;
}
}
struct MotorParams
{
uint8 index;
float angle;
};
void Poly_servo_goto(float angle1, float angle2, struct MotorParams* motor_params)
{
if (angle1 >= angle2)
{
motor_params->index = 0;
motor_params->angle = angle1;
}
else
{
motor_params->index = 1;
motor_params->angle = angle2;
}
}
void Poly_servo_goto(float angles[], int num_angles, uint8* result_index)
{
uint8 index = 0;
float max_angle = 0;
for (int i=0; i<num_angles; ++i)
{
if (angles[i] >= max_angle)
{
max_angle = angles[i];
index = (uint8)i;
}
}
*result_index = index;
}
Some other advices: Don't use smaller integers than int/unsigned like (uint8) if you don't want to save space in memory. The most effective integer on a platform is usually an integer that has to same width as the registers of the processor (on a 32 bit system its int32). int/unsigned usually satisfies this requirement (on 16 and 32 bit platforms, not on 64 bit ones). When you pass parameters on the stack and you store them in local variables it usually still takes the space of a machine-word (32 bit on 32bit system) for many reasons so using an int/uint is often a nice solution and the code nicer.