runDeviceSimulate.cpp

#include "encCtrl.h"
#include "motorCtrl.h"
#include "directDeviceCtrl.h"
#include "wheelCtrl.h"
#include "sh7045lib.h"
#include <math.h>
#include <stdlib.h>


enum {
  MAX_STRAIGHT_VELOCITY = 650,
};

static int EncRef[2] = { 0, 0 };
static int MtrMode[2] = { DIRECT_MODE_NONE, DIRECT_MODE_NONE };


void initEnc(void) {
  for (int i = 0; i < 2; ++i) {
    EncRef[i] = 0;
  }
}


void initEncInfo(unsigned char id, encInfo_t *enc) {
  enc->id = id;
  enc->prev = 0;
  enc->diff = 0;
  enc->enc_direction = ENC_CW_TO_PLUSE;
}


void updateEncDiff(encInfo_t *enc) {
  // エンコーダ値の取得
  enc->diff = EncRef[enc->id];
}


void set_mode(const unsigned char id, const int mode) {
  MtrMode[id] = mode;
}


static void setDirectPwmVel(int id, int duty, int mode) {
  int enc_ref = duty - 5 + static_cast<int>(7.0 * rand()/(RAND_MAX+1.0) -3);
  enc_ref = (enc_ref < 0) ? 0 : enc_ref;

  EncRef[id] = ((mode == DIRECT_MODE_CW) ? +1 : -1) * enc_ref;
}


void set_pwm(const unsigned char id, const int duty) {
  switch (MtrMode[id]) {

  case DIRECT_MODE_CW:
    setDirectPwmVel(id, duty, DIRECT_MODE_CW);
    break;

  case DIRECT_MODE_CCW:
    setDirectPwmVel(id, duty, DIRECT_MODE_CCW);
    break;

  case DIRECT_MODE_FREE:
    EncRef[id] = (EncRef[id] * 3) >> 2;
    break;
  }
}


void initMotor(void) {
  for (int i = 0; i < 2; ++i) {
    EncRef[i] = 0;
  }
}


void initMotorInfo(unsigned char id, motorInfo_t *mtr) {
  mtr->id = id;
}


void setMotorFree(motorInfo_t *mtr) {
  EncRef[mtr->id] = (EncRef[mtr->id] * 3) >> 2;
}


int setMotorRevolution(const int ref_cnt, const int cnt_diff,
                       motorInfo_t *mtr) {
  const static int max_cnt =
    (int)(2 * MAX_STRAIGHT_VELOCITY * WHL_GEAR_RATIO
          / (M_PI * WHL_DIAMETER_RIGHT));
  int cnt;

  // エンコーダ値の設定
  if (abs(ref_cnt) > max_cnt) {
    // 筐体の移動速度を越えるカウンタ値は処理しない
    cnt = (ref_cnt < 0) ? -max_cnt : +max_cnt;
  } else {
    cnt = ref_cnt;
  }
  EncRef[mtr->id] = cnt;
  return 0;
}


void setDirectMotorMode(unsigned char id, int mode) {
}

void setDirectMotorPwm(unsigned char id, int duty) {
}

void getRawCount(unsigned short *cnt) {
  *cnt = 0;
}