motorCtrl.c

Go to the documentation of this file.

#include "motorCtrl.h"
#include "encCtrl.h"
#ifdef SIMULATOR
enum { SMP_USEC = 1000 };
void set_mode(const unsigned char id, const int mode) {}
void set_pwm(const unsigned char id, const int duty) {}
#else
#include <7040S.H>

#include "tRunCtrl.h"
#ifndef SMP_USEC
#define SMP_USEC 1000
#endif


void set_pwm(const unsigned char id, const int duty) {

  if (id == 0) {
    if (duty <= 0 || duty >= 255) {
      PFC.PECR1.WORD &= ~0x0001; /* PE8 */
      PE.DR.WORD &= ~0x0100;
      PE.DR.WORD |= (duty <= 0) ? 0 : 0x0100;
    } else {
      PFC.PECR1.WORD |= 0x0001; /* TIOC3A */
      MTU3.TGRB = duty;
    }
  } else if (id == 1) {
    if (duty <= 0 || duty >= 255) {
      PFC.PECR1.WORD &= ~0x0010; /* PE10 */
      PE.DR.WORD &= ~0x0400;
      PE.DR.WORD |= (duty <= 0) ? 0 : 0x0400;
    } else {
      PFC.PECR1.WORD |= 0x0010; /* TIOC3C */
      MTU3.TGRD = duty;
    }
  }
}


void set_mode(const unsigned char id, const int mode) {
  if (id == 0) {
    /* DIR(PE5), EN(PE7) */
    if (mode == MTR_MODE_CCW_BREAK) {
      PE.DR.WORD |= 0x0020;     /* DIR(PE5)=1 */
      PE.DR.WORD &= ~0x0080;    /* EN(PE7)=0 */
    } else if (mode == MTR_MODE_CW_BREAK) {
      PE.DR.WORD &= ~0x0020;    /* DIR(PE5)=0 */
      PE.DR.WORD &= ~0x0080;    /* EN(PE7)=0 */
    } else {                    /* MTR_MODE_FREE */
      PE.DR.WORD |= 0x0080;     /* EN(PE7)=1 */
    }

  } else if (id == 1) {
    /* DIR(PE9), EN(PE11) */
    if (mode == MTR_MODE_CCW_BREAK) {
      PE.DR.WORD |= 0x0200;     /* DIR(PE9)=1 */
      PE.DR.WORD &= ~0x0800;    /* EN(PE11)=1 */
    } else if (mode == MTR_MODE_CW_BREAK) {
      PE.DR.WORD &= ~0x0200;    /* DIR(PE9)=0 */
      PE.DR.WORD &= ~0x0800;    /* EN(PE11)=1 */
    } else {                    /* MTR_MODE_FREE */
      PE.DR.WORD |= 0x0800;     /* EN(PE11)=1 */
    }
  }
}


// 直接制御用
void setDirectMotorMode(unsigned char id, int mode) {
  set_mode(id, mode);
}

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


void initMotor(void) {
  int i;

  PFC.PEIOR.WORD |= 0x0500;     /* use TIOC3A, TIOC3C port */
  MTU3.TCR.BYTE = 0x20 | 0x01;  /* cycle=TGRA, duty=TGRB, cycle/4 */
  MTU3.TMDR.BYTE = 0xc2;        /* pwm mode 1, buffer disable */
  MTU3.TIOR.BYTE.H = 0x12;      /* CMA=0, CMB=1 */
  MTU3.TIOR.BYTE.L = 0x12;      /* CMC=0, CMD=1 */
  MTU3.TGRA = MTU3.TGRC = 255;  /* cycle */

  for (i = 0; i <= 1; ++i) {
    set_pwm(i, 0);
    set_mode(i, MTR_MODE_FREE);
  }

  PFC.PECR1.WORD |= 0x0011;     /* use TGRA, TGRC port */

  MTU.TSTR.BYTE |= 0xc0;        /* pwm active */

  PFC.PECR1.WORD &= ~0x0044;    /* TIOC3B, TIOC3D */
  PFC.PECR2.WORD &= ~0x4400;    /* TIOC1B, TIOC2B */
  PFC.PEIOR.WORD |= 0x0aa0;
}
#endif


void initMotorInfo(const unsigned char id, motorInfo_t *mtr) {
  mtr->id = id;
  mtr->gain_p = MTR_GAIN_P;
  mtr->gain_i = MTR_GAIN_I;
  mtr->i_value = 0;

  mtr->r_per_v = (int)((1<<MTR_I_BIT_WIDTH) * MTR_OHM / MTR_POWER);
  mtr->ke_per_v = (int)(1.0 / MTR_RPM_PER_V
                        * 60 * 1000 / (SMP_USEC / 1000)
                        * (1 << MTR_I_BIT_WIDTH) / (MTR_POWER * ENC_PULSE));
}


static int calcGainOutput(const int ref_cnt, const int cnt_diff,
                          motorInfo_t *mtr) {
  int diff;
  int output;

  diff = ref_cnt - cnt_diff;
  mtr->i_value += diff;
  if (mtr->i_value > I_VALUE_MAX) {
    mtr->i_value = I_VALUE_MAX;
  } else if (mtr->i_value < -I_VALUE_MAX) {
    mtr->i_value = -I_VALUE_MAX;
  }
  output = (mtr->gain_p * diff) + (mtr->gain_i * mtr->i_value);
  output <<= (MTR_I_BIT_WIDTH - 10);

  return output;
}


// モータに対して、MTR_I_BIT_SHIF シフトされた電流[A] を流すように設定する
static int calcInvertedCtrlPwm(const int output, const int cnt_diff,
                               motorInfo_t *mtr) {

  int sign;
  int pwm;

  pwm = (mtr->r_per_v * output) + (mtr->ke_per_v * cnt_diff);
  sign = (pwm < 0) ? -1 : +1;
  pwm = (sign * pwm) >> ((MTR_I_BIT_WIDTH << 1) - 8);
  if (pwm > 0xff) {
    pwm = 0xff;
  }
  return (sign * pwm);
}


void setMotorFree(motorInfo_t *mtr) {
  set_mode(mtr->id, MTR_MODE_FREE);
}


int setMotorRevolution(const int ref_cnt, const int cnt_diff,
                       motorInfo_t *mtr) {
  int ref_pwm;
  int mode;
  int pwm;
  int output;

  output = calcGainOutput(ref_cnt, cnt_diff, mtr);
  ref_pwm = calcInvertedCtrlPwm(output, cnt_diff, mtr);
  mode = (ref_pwm >= 0) ? MTR_MODE_CW_BREAK : MTR_MODE_CCW_BREAK;
  pwm = (ref_pwm >= 0) ? ref_pwm : -ref_pwm;

  set_pwm(mtr->id, pwm);
  set_mode(mtr->id, mode);

  return ref_pwm;
}