velocity.cpp

/*
  車輪の直接制御により速度制御を行うプログラム
  Satofumi KAMIMURA
  $Id$

  C言語っぽい実装
*/

#include "runCtrl.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <signal.h>

enum {
  Right = 0, Left = 1,
  DefaultAcc = 100,             // 標準加速度 [mm/sec]
};

typedef void (callback_t)(int);

static RunCtrl Run;
static int EncRefVel = 0;       // エンコーダ単位の目標速度[cnt/msec^2]
static int EncRefAcc = DefaultAcc; // エンコーダ単位の目標加速度 [cnt/sec^2]
static int TotalTicks = 0;      // 制御開始からの時刻 [msec]
static int PreTicks = 0;        // 前回の時刻 [msec]


static void setIntervalTimer(callback_t *func, int interval_msec) {
  struct itimerval itimer;
  struct sigaction act;
  
  // initilaize interval tiemr
  itimer.it_value.tv_sec = 0;
  itimer.it_value.tv_usec = interval_msec * 1000;
  itimer.it_interval.tv_sec = 0;
  itimer.it_interval.tv_usec = interval_msec * 1000;
  setitimer(ITIMER_REAL, &itimer, NULL);

  // set signal handler
  act.sa_handler = func;
  sigemptyset(&act.sa_mask);
  act.sa_flags = 0;

  sigaction(SIGALRM, &act, 0);
}


static void stopIntervalTimer(void) {
  struct itimerval itimer;
  itimer.it_value.tv_sec = 0;
  itimer.it_value.tv_usec = 0;
  setitimer(ITIMER_REAL, &itimer, NULL);
}


static int getTicks(void) {
  struct timeval time;
  gettimeofday(&time, NULL);
  return (time.tv_sec * 1000) + (time.tv_usec / 1000);
}


static void setRefVelocity(int mm_sec_vel, int mm_sec_acc = DefaultAcc) {
  // 目標速度をエンコーダ単位系での目標速度[cnt/msec]に変換

  EncRefVel = static_cast<int>(mm_sec_vel * WHL_CNT_PER_MM_RIGHT / 1000);
  EncRefAcc = static_cast<int>(mm_sec_acc * WHL_CNT_PER_MM_RIGHT / 1000);

  printf("EncRefVel: %d, EncRefAcc: %d\n", EncRefVel, EncRefAcc);
}


static void initVelocityCtrl(void) {
  PreTicks = getTicks();
}


static int calcPwmValue(int EncRefVel, int* enc_vel, int* diff_total) {
  double Gain_P = 0.000004;
  double Gain_I = 0.00001;

  int diff = EncRefVel - *enc_vel;
  *diff_total += diff;

  double output = (Gain_P * diff) + (Gain_I * *diff_total);
  double duty = output * 255.0;

  int pwm = (fabs(duty) > 1.0) ?
    static_cast<int>(255 * duty/fabs(duty)) : static_cast<int>(255 * duty);

  return static_cast<int>(pwm);
}


static void velocityCtrl(int sig) {
  static bool initialized = false;
  if (!initialized) {
    initialized = true;
    initVelocityCtrl();
    return;
  }
  static int pwm[2];
  static int diff_total[2] = { 0, 0 };

  // 前回からの経過時間を取得
  int now = getTicks();
  int interval_msec = now - PreTicks;
  TotalTicks += interval_msec;
  PreTicks = now;
  printf("ticks: %d (%d) [msec]\n", TotalTicks, interval_msec);

  // エンコーダ値の読みだし
  int enc_vel[2];
  for (int i = 0; i < 2; ++i) {
    //Run.getEncoderVel(i, &enc_vel[i]);
    enc_vel[i] *= -1;
  }

  // PWM の演算
  int ref_enc_vel = TotalTicks * EncRefAcc;
  ref_enc_vel = (ref_enc_vel > EncRefVel) ? EncRefVel : ref_enc_vel;
  for (int i = 0; i < 2; ++i) {
    pwm[i] = calcPwmValue(ref_enc_vel, &enc_vel[i], &diff_total[i]);
  }

  // モータへの回転指示
  for (int i = 0; i < 2; ++i) {
    //int mode = (pwm[i] >= 0) ? DIRECT_MODE_CCW : DIRECT_MODE_CW;
    //Run.setMotorMode(i, mode);
    //Run.setMotorPwm(i, abs(pwm[i]));
  }

  printf("ref_enc_vel: %d\n", ref_enc_vel);
  printf("enc_vel: %d, %d\n", enc_vel[Right], enc_vel[Left]);
  printf("pwm: %d, %d\n", pwm[Right], pwm[Left]);
  printf("\n");
}


int main(int argc, char *argv[]) {
  try {
    enum { IntervalMsec = 130 };

    // ライブラリの初期化
    if (Run.connect(argc, argv) < 0) {
      printf("RunCtrl::connect: %s\n", Run.what());
      exit(1);
    }

    // 初期化
    setRefVelocity(100);
    setIntervalTimer(velocityCtrl, IntervalMsec);

    // 改行で停止
    while (getchar() == EOF) {
      sleep(1);
    }

    stopIntervalTimer();
    Run.stop();
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}