TIPS: nxtway_gs_balancerのturn値処理にはバグがある

これは ETロボコン Advent Calendar 2014 - Adventar の2日目のエントリーです。

東海地区技術委員の清水さん( k.shimizu (@pulmaster2) | Twitter )より東海地区独自教育「初心者向け教育」の時に伺ったお話。
balance_control関数はバランスを取るためのPWM値を計算した後、turn値より計算した値を左に足し、右から引く事で旋回する。ただしPWM値は-100～100の範囲しか取れないので、超えないように制限をかける。この時、せっかくバランスが取れるように同じ値を左右に足し引きするのに、その後で片方だけ制限するのでバランスが崩れてしまうよ、という事でした。
例えば今、前に傾いているのでバランスを取るために左右のPWMが80必要として、turn値から計算した値が-40とすると左は80-40で40になるけど右は80+40が100を超えてしまうので100になり、左から40引いて右に20足した状態になってしまう。

具体的なコードの場所はbalancer.cの267行目前後から。

    tmp_pwm_r_limiter = (((K_I * ud_err_theta) + tmp_pwm_r_limiter) /
                         ((BATTERY_GAIN * args_battery) - BATTERY_OFFSET)) *
      100.0F;

    /* Gain: '<S3>/Gain2' incorporates:
     *  Constant: '<S3>/Constant1'
     *  Inport: '<Root>/cmd_turn'
     *  Product: '<S3>/Divide1'
     */
    tmp_pwm_turn = (args_cmd_turn / CMD_MAX) * K_PHIDOT;

    /* Sum: '<S2>/Sum' */
    tmp_pwm_l_limiter = tmp_pwm_r_limiter + tmp_pwm_turn;

    /* Saturate: '<S2>/pwm_l_limiter' */
    tmp_pwm_l_limiter = rt_SATURATE(tmp_pwm_l_limiter, -100.0F, 100.0F);

    /* Outport: '<Root>/pwm_l' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion'
     */
    (*ret_pwm_l) = (S8)tmp_pwm_l_limiter;

    /* Sum: '<S2>/Sum1' */
    tmp_pwm_r_limiter -= tmp_pwm_turn;

    /* Saturate: '<S2>/pwm_r_limiter' */
    tmp_pwm_r_limiter = rt_SATURATE(tmp_pwm_r_limiter, -100.0F, 100.0F);

    /* Outport: '<Root>/pwm_r' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion6'
     */
    (*ret_pwm_r) = (S8)tmp_pwm_r_limiter;

そんなわけで、balance_control関数は常にturn=0でコールして、その後で左右のモーターのPWM絶対値が100を超えないようなturn量を左右に足し引きするようにしたところ、特に高速走行が格段に安定するようになりました。

おしまい。