velocityCtrl.c

Go to the documentation of this file.

#include "velocityCtrl.h"
#include "isqrt.h"
#include "iatan2.h"
#include "isincos.h"


void initVelocityCtrlInfo(velocityCtrlInfo_t *velCtrl) {
  velCtrl->ref_vel = 0;
  velCtrl->current_vel = 0;
  velCtrl->acc = 0;
  velCtrl->current_pos = 0;
  velCtrl->circle_r = 500;
  velCtrl->stable = CONTROL_UNSTABLE;
}


void getFirstHoldVelocity(velocityCtrlInfo_t *velCtrl, int vel) {
  velCtrl->current_vel = vel * 1000;
}


/* 速度制御 */
int holdVelocityCtrl(velocityCtrlInfo_t *velCtrl) {
  int diff = (velCtrl->ref_vel * 1000) - velCtrl->current_vel;

  if (diff > +velCtrl->acc) {
    velCtrl->current_vel += velCtrl->acc;
  } else if (diff < -velCtrl->acc) {
    velCtrl->current_vel -= velCtrl->acc;
  } else {
    velCtrl->stable = CONTROL_STABLE;
    return velCtrl->ref_vel;
  }

  return velCtrl->current_vel / 1000;
}


/* 位置制御 */
int holdPositionCtrl(velocityCtrlInfo_t *velCtrl, int stable_shift) {
  int pos_sign, pos_abs;
  int vel_sign, vel_abs;
  int ret_value;
  int store_ref_vel = velCtrl->ref_vel;

  pos_sign = (velCtrl->current_pos < 0) ? -1 : +1;
  pos_abs = pos_sign * velCtrl->current_pos;
  vel_sign = (velCtrl->current_vel < 0) ? -1 : +1;
  vel_abs = vel_sign * velCtrl->current_vel / 1000;

  if ((isqrt(velCtrl->acc << 1) * isqrt(pos_abs)) > (unsigned)(vel_abs)) {
    velCtrl->ref_vel = - pos_sign * velCtrl->ref_vel;
  } else {
    velCtrl->ref_vel =
      - pos_sign * isqrt(velCtrl->acc << 1) * isqrt(pos_abs);
  }
  ret_value = holdVelocityCtrl(velCtrl);
  velCtrl->ref_vel = store_ref_vel;
  velCtrl->stable = ((pos_abs >> stable_shift) == 0) ?
    CONTROL_STABLE : CONTROL_UNSTABLE;

  if ((isqrt(velCtrl->acc << 1) * isqrt(pos_abs)) > (unsigned)(vel_abs)) {
    return ret_value;
  } else {
    return (1000 * ret_value) >> 10;
  }
}


int followLineCtrl(velocityCtrlInfo_t *velCtrl, int follow_r,
                   velocityInfo_t *velInfo, int run_crd_div16) {
  int set_current_pos;
  int l = velCtrl->current_pos;
  int l_sign, l_abs;
  int vel_sign = (velInfo->straight_mm_sec_vel < 0) ? -1 : +1;
  int r = follow_r;
  int rl_diff;
  int ret_value;

  l_sign = (l < 0) ? -1 : +1;
  l_abs = (l < 0) ? -l : +l;

  if (l_abs >= r) {
    set_current_pos = l_sign * (0x10000 >> 2);
  } else {
    int vel_adjust =
      l_sign * iatan2(vel_sign * velInfo->straight_mm_sec_vel, r);
    rl_diff = (l_sign > 0) ? r - l : r + l;
    set_current_pos =
      l_sign * ((0x10000 >> 2)
                - vel_sign * iatan2(rl_diff, isqrt(r*r - rl_diff*rl_diff)));

    // 並進速度の補正
    set_current_pos -= vel_sign * vel_adjust >> 1;
  }

  // 後進時
  if (vel_sign < 0) {
    set_current_pos -= l_sign * (0x10000 >> 1);
  }

  // 目標直線を越えないための補正
  if (l_abs < (r >> 2)) {
    set_current_pos >>= 1;
    if (l_abs < (r >> 5)) {
      set_current_pos >>= 1;
    }
  }

  velCtrl->current_pos =
    getSignedDiv16((set_current_pos + run_crd_div16) & 0xffff);
  ret_value = holdPositionCtrl(velCtrl, DIV16_STABLE_SHIFT);
  if (velCtrl->stable == CONTROL_STABLE) {
    velCtrl->stable = ((l_abs >> (MM_STABLE_SHIFT + 2)) == 0)
      ? CONTROL_STABLE : CONTROL_UNSTABLE;
  }
  return ret_value;
}


int followCircleCtrl(velocityCtrlInfo_t *velCtrl, int follow_r, int x, int y,
                     velocityInfo_t *velInfo, int run_crd_div16) {
  int r = isqrt(x*x + y*y);
  int sign = (velCtrl->circle_r < 0) ? -1 : +1;
  int offset = iatan2(y, x);
  int ret_value;
  int diff = 0;

  velCtrl->current_pos = (sign * r) - velCtrl->circle_r;
  if (velCtrl->circle_r != 0) {
    diff = - (int)(0x10000 / (2 * M_PI))
      * velInfo->straight_mm_sec_vel / velCtrl->circle_r;
  }
  ret_value = followLineCtrl(velCtrl, follow_r, velInfo,
                             run_crd_div16
                             - getSignedDiv16(offset - sign * (0x10000 >> 2)));

  return ret_value + diff;
}