image.png

    给定四个狗腿的坐标,计算8个舵机转动角度 需要考虑左右腿的安装角度

    左腿
    image.png
    右腿
    image.png

    1. //特定位置的舵机角度
    2. void leg_inverse_angle(double rq_x,double rq_y,double rh_x,double rh_y,
    3.     double lq_x,double lq_y,double lh_x,double lh_y,double steering_angles[8]){
    5.     //右前
    6.     leg_inverse(rq_x,rq_y,&alpha,&beta);
    7.     steering_angles[0] = rad2deg(PI-beta);
    8.     steering_angles[1] = rad2deg(alpha);
    9.     //printf("dj1:%lf ,dj2:%lf\n",steering_angles[0],steering_angles[1]);
    10.     //右后
    11.     leg_inverse(rh_x,rh_y,&alpha,&beta);
    12.     steering_angles[2] = rad2deg(PI-beta);
    13.     steering_angles[3] = rad2deg(alpha);
    14.     //printf("dj3:%lf ,dj4:%lf\n",steering_angles[2],steering_angles[3]);
    15.     //左前
    16.     leg_inverse(lq_x,lq_y,&alpha,&beta);
    17.     steering_angles[4] = rad2deg(beta);
    18.     steering_angles[5] = rad2deg(PI-alpha);
    19.     //printf("dj5:%lf ,dj6:%lf\n",steering_angles[4],steering_angles[5]);
    20.     //左后
    21.     leg_inverse(lh_x,lh_y,&alpha,&beta);
    22.     steering_angles[6] = rad2deg(beta);
    23.     steering_angles[7] = rad2deg(PI-alpha);
    24.     //printf("dj7:%lf ,dj8:%lf\n",steering_angles[6],steering_angles[7]);
    25. }

    狗腿运动到指定的位置

    1. //狗运动到指定的位置
    2. void dog_to_coord(double rq_x,double rq_y,double rh_x,double rh_y,
    3.     double lq_x,double lq_y,double lh_x,double lh_y){
    5.     leg_inverse_angle(rq_x, rq_y, rh_x, rh_y,lq_x, lq_y, lh_x, lh_y, steering_angles);
    7.     bsp_pwm0_angle(steering_angles[0]);
    8.     bsp_pwm1_angle(steering_angles[1]); 
    9.     bsp_pwm2_angle(steering_angles[2]);
    10.     bsp_pwm3_angle(steering_angles[3]);
    11.     bsp_pwm4_angle(steering_angles[4]);
    12.     bsp_pwm5_angle(steering_angles[5]);
    13.     bsp_pwm6_angle(steering_angles[6]);
    14.     bsp_pwm7_angle(steering_angles[7]);
    15. }