0
$\begingroup$

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;
}
$\endgroup$
3
  • $\begingroup$ 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 $\endgroup$
    – Wilhelm
    Dec 29, 2022 at 18:15
  • $\begingroup$ Hi Dave, the full code is updated. $\endgroup$
    – huiduan
    Dec 30, 2022 at 7:45
  • $\begingroup$ 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? $\endgroup$
    – Wilhelm
    Dec 30, 2022 at 11:08

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.