# How to use the piecewise function to control the rotation motor in webots?

I am creating a Webots robot controller by using C language and trying to use piecewise functions to control two rotation motors and simulate the swinging motion of a leg. One motor for the hip and another one for the knee. However, the motors only work with the "else" condition. Can anyone tell me where I am wrong?

This is the code:

#include <webots/robot.h>
#include <webots/motor.h>
#include <math.h>

#define TIME_STEP 64

int main(int argc, char **argv) {
wb_robot_init();

WbDeviceTag Right_Hip = wb_robot_get_device("RightHip");
WbDeviceTag RightUpperKnee = wb_robot_get_device("RightUpperKnee");

double t = 0.0;  // elapsed simulation time
int k = 0;

while (wb_robot_step(TIME_STEP) != -1) {
double RightHipPosition;
double RightUpperKneePosition;

if ((t >= (0 + 4 * k * M_PI)) && (t <= (12/5 * M_PI + 4 * k * M_PI)))
RightHipPosition = (35 * cos(0.417* t )+ 30)* (M_PI / 180.0);
else
RightHipPosition = (35 * sin((0.625) * t )+30) * (M_PI / 180.0);

if ((t >= (0 + 4 * k * M_PI)) && (t <= (6.241 + 4 * k * M_PI)))
RightUpperKneePosition= -30 * (M_PI / 180.0);
else if ((t >= (6.241 + 4 * k * M_PI)) && (t <= (2.4 * M_PI + 4* k * M_PI)))
RightUpperKneePosition =(30 * sin (0.417 * t ) )  * (M_PI / 180.0);
else
RightUpperKneePosition =(-30 * sin (0.675 * t ) )  * (M_PI / 180.0);

wb_motor_set_position(Right_Hip, RightHipPosition);
wb_motor_set_position(RightUpperKnee, RightUpperKneePosition);

t += (double)TIME_STEP / 1000.0;
k = k + 1;
}
wb_robot_cleanup();
return 0;
}

• Your code is not complete. Where are the variables t and k defined? Which else branch is going to be executed? Put something like: print("I'm here: A") in the first elsebranch and so on for the others Dec 29, 2022 at 18:15
• Hi Dave, the full code is updated. Dec 30, 2022 at 7:45
• By the way: you should post which compiler and environment you are using, otherwise we can get different results. As a rule of thumb, you should handle float calculations as float or double. So for instance: t >= (0.0 + 4.0 * k * M_PI)) && (t <= (12.0/5.0 * M_PI + 4.0 * k * M_PI) . Note the .0after the integers. The same for the variable k, which is an integer. Furhtermore, why you add 0 to 4 * k* M_PIit is not clear to me. My suggestion: start with the only first ifbranch and print the result of the calculation. If it makes sense, then you can add the second if statement, ok? Dec 30, 2022 at 11:08