wheelCtrl.c

Go to the documentation of this file.

#include "wheelCtrl.h"

//#include <stdio.h>


int change_wheelVel2Cnt(wheelInfo_t *whl, int mm_sec_vel) {

  int cnt_msec_vel = (whl->cnt2vel_const * mm_sec_vel) >> 16;
  return cnt_msec_vel;
}


int change_wheelCnt2Vel(wheelInfo_t *whl, int cnt_msec_vel) {

  int mm_sec_vel = (whl->vel2cnt_const * cnt_msec_vel) >> 16;
  return mm_sec_vel;
}


void initWheelInfo(wheelInfo_t *whl,
                   motorInfo_t *mtr, encInfo_t *enc, const unsigned char id) {
  whl->mtr = mtr;
  whl->enc = enc;
  whl->next_add = 0;
  if (id == WHL_RIGHT) {
    whl->cnt2vel_const = WHL_VEL2CNT_16SHIFT_RIGHT;
    whl->vel2cnt_const = WHL_CNT2VEL_16SHIFT_RIGHT;
    whl->cnt_per_m = WHL_CNT_PER_M_RIGHT;
  } else {
    whl->cnt2vel_const = WHL_VEL2CNT_16SHIFT_LEFT;
    whl->vel2cnt_const = WHL_CNT2VEL_16SHIFT_LEFT;
    whl->cnt_per_m = WHL_CNT_PER_M_LEFT;
  }
}


void setWheelFree(wheelInfo_t *whl) {
  setMotorFree(whl->mtr);
}


void setWheelMoveVelocity(int mm_sec_vel, wheelInfo_t *whl) {

  // mm/sec 系の速度を cnt/msec 系の速度に変換
  int ref_mm_sec_vel = mm_sec_vel + whl->next_add;
  int cnt_msec_vel = change_wheelVel2Cnt(whl, ref_mm_sec_vel);
  whl->next_add = ref_mm_sec_vel - change_wheelCnt2Vel(whl, cnt_msec_vel);

  // 指定速度をモータに指示
  //fprintf(stderr, "whl vel: %d\n", cnt_msec_vel);
  setMotorRevolution(cnt_msec_vel, whl->enc->diff, whl->mtr);
}