bodyCtrl.c

Go to the documentation of this file.

#include "bodyCtrl.h"
#include <math.h>

//#include <stdio.h>


void initBodyInfo(bodyInfo_t *body, wheelInfo_t *right, wheelInfo_t *left) {
  body->whl[BODY_WHL_RIGHT] = right;
  body->whl[BODY_WHL_LEFT] = left;
  body->tread_mm = (int)(BODY_TREAD_MM);
  body->div2vel_const =
    (int)((1 << DIV2VEL_CONST_SHIFT) * BODY_TREAD_MM * M_PI);

  // ジャンパを正常に取り付けた場合
  body->mtr_direction[BODY_WHL_RIGHT] = DEFAULT_WHL_RIGHT;
  body->mtr_direction[BODY_WHL_LEFT] = DEFAULT_WHL_LEFT;
}


int change_bodyDiv2Whl(int div16_vel, bodyInfo_t *body) {

  int whl_mm_vel =
    (div16_vel * body->div2vel_const) >> (16 + DIV2VEL_CONST_SHIFT);

  return whl_mm_vel;
}


void setBodyMove(bodyInfo_t *body, int straight_mm_vel, int rotate_div16_vel) {
  int i;

  int rotate_mm_vel = change_bodyDiv2Whl(rotate_div16_vel, body);
  int whl_mm_vel[2];
  whl_mm_vel[WHL_RIGHT] = straight_mm_vel + rotate_mm_vel;
  whl_mm_vel[WHL_LEFT] = straight_mm_vel - rotate_mm_vel;

  //fprintf(stderr, "%d, %d\n", whl_mm_vel[WHL_LEFT], whl_mm_vel[WHL_RIGHT]);
  //fprintf(stderr, "%d, %d\n", whl_mm_vel[0], whl_mm_vel[1]);

  for (i = 0; i < 2; ++i) {
    setWheelMoveVelocity(body->mtr_direction[i] * whl_mm_vel[i], body->whl[i]);
  }
}