modeCtrl.c

Go to the documentation of this file.

#include "modeCtrl.h"


void initModeInfo(modeInfo_t *mode) {
  mode->ctrl = MODE_SERVO_FREE;
  mode->straight_ctrl = MODE_HOLD_POSITION_FIRST;
  mode->rotate_ctrl = MODE_HOLD_ANGLE_FIRST;
  mode->pre_straight_ctrl = mode->straight_ctrl;
  mode->pre_rotate_ctrl = mode->rotate_ctrl;
}


/* 並進速度の計算 */
int calcStraightRefVel(velocityCtrlInfo_t *velCtrl, modeInfo_t *mode,
                       bodyPosition_t *bodyPos, velocityInfo_t *velInfo,
                       coordinateInfo_t *run_crd) {
  int straight_mm_vel = 0;
  velCtrl->stable = CONTROL_UNSTABLE;

  switch (mode->straight_ctrl) {

  case MODE_HOLD_VELOCITY_FIRST:
    if (mode->pre_straight_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->straight_mm_sec_vel);
    }
    mode->straight_ctrl = MODE_HOLD_VELOCITY;
  case MODE_HOLD_VELOCITY:
    straight_mm_vel = holdVelocityCtrl(velCtrl);
    break;

  case MODE_HOLD_POSITION_FIRST:
    if (mode->pre_straight_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->straight_mm_sec_vel);
    }
    mode->straight_ctrl = MODE_HOLD_POSITION;
  case MODE_HOLD_POSITION:
    get_mmCoordinate(run_crd, &velCtrl->current_pos, NULL);
    straight_mm_vel = holdPositionCtrl(velCtrl, MM_STABLE_SHIFT);
    break;

  case MODE_STOP_FIRST:
    if (mode->pre_straight_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->straight_mm_sec_vel);
    }
    mode->straight_ctrl = MODE_STOP;
  case MODE_STOP:
    velCtrl->ref_vel = 0;
    straight_mm_vel = holdVelocityCtrl(velCtrl);
    break;

  case MODE_NO_CTRL:
  default:
    straight_mm_vel = 0;
    velCtrl->stable = CONTROL_STABLE;
    break;
  }

  mode->pre_straight_ctrl = mode->straight_ctrl;
  return straight_mm_vel;
}


/* 回転速度の計算 */
int calcRotateRefVel(velocityCtrlInfo_t *velCtrl, modeInfo_t *mode,
                     bodyPosition_t *bodyPos, velocityInfo_t *velInfo,
                     coordinateInfo_t *run_crd) {
  int x, y;
  int rotate_div16_vel = 0;
  velCtrl->stable = CONTROL_UNSTABLE;

  switch (mode->rotate_ctrl) {

  case MODE_HOLD_ROTATE_VELOCITY_FIRST:
    if (mode->pre_rotate_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->rotate_div16_sec_vel);
    }
    mode->rotate_ctrl = MODE_HOLD_ROTATE_VELOCITY;
  case MODE_HOLD_ROTATE_VELOCITY:
    rotate_div16_vel = holdVelocityCtrl(velCtrl);
    break;

  case MODE_HOLD_ANGLE_FIRST:
    if (mode->pre_rotate_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->rotate_div16_sec_vel);
    }
    mode->rotate_ctrl = MODE_HOLD_ANGLE;
  case MODE_HOLD_ANGLE:
    velCtrl->current_pos = getSignedDiv16(run_crd->div16);
    rotate_div16_vel = holdPositionCtrl(velCtrl, DIV16_STABLE_SHIFT);
    break;

  case MODE_ROTATE_ANGLE_FIRST:
    if (mode->pre_rotate_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->rotate_div16_sec_vel);
    }
    velCtrl->circle_r += (bodyPos->rotate_num << 16) + bodyPos->body_crd.div16;
    mode->rotate_ctrl = MODE_ROTATE_ANGLE;
  case MODE_ROTATE_ANGLE:
    velCtrl->current_pos =
      ((bodyPos->rotate_num << 16) + bodyPos->body_crd.div16)
      - velCtrl->circle_r;
    rotate_div16_vel = holdPositionCtrl(velCtrl, DIV16_STABLE_SHIFT);
    break;

  case MODE_FOLLOW_LINE_FIRST:
    if (mode->pre_rotate_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->rotate_div16_sec_vel);
    }
    velCtrl->circle_r = mode->follow_r;
    mode->rotate_ctrl = MODE_FOLLOW_LINE;
  case MODE_FOLLOW_LINE:
    get_mmCoordinate(run_crd, NULL, &velCtrl->current_pos);
    rotate_div16_vel = followLineCtrl(velCtrl, mode->follow_r,
                                      velInfo, run_crd->div16);
    break;

  case MODE_FOLLOW_CIRCLE_FIRST:
    if (mode->pre_rotate_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->rotate_div16_sec_vel);
    }
    mode->rotate_ctrl = MODE_FOLLOW_CIRCLE;
  case MODE_FOLLOW_CIRCLE:
    get_mmCoordinate(run_crd, &x, &y);
    rotate_div16_vel =
      followCircleCtrl(velCtrl, mode->follow_r, x, y, velInfo, run_crd->div16);
    break;

  case MODE_ROTATE_STOP_FIRST:
    if (mode->pre_rotate_ctrl == MODE_NO_CTRL) {
      getFirstHoldVelocity(velCtrl, velInfo->rotate_div16_sec_vel);
    }
    mode->straight_ctrl = MODE_ROTATE_STOP;
  case MODE_ROTATE_STOP:
    velCtrl->ref_vel = 0;
    rotate_div16_vel = holdVelocityCtrl(velCtrl);
    break;

  case MODE_NO_CTRL:
  default:
    rotate_div16_vel = 0;
    velCtrl->stable = CONTROL_STABLE;
    break;
  }

  mode->pre_rotate_ctrl = mode->rotate_ctrl;
  return rotate_div16_vel;
}