/*!
  \example motor_velocity.c モータの速度制御サンプル

  \author Satofumi KAMIMURA

  $Id: motor_velocity.c 1677 2010-02-07 13:59:59Z satofumi $
*/

#include "led_control.h"
#include "sci_control.h"
#include "sci_put.h"
#include "mtu_control.h"
#include "cmt_control.h"
#include "encoder_control.h"
#include "motor_control.h"
#include "pwm_control.h"
#include "imask_control.h"

enum {
  RecordTimes = 300,
};

static encoder_t encoder_;
static motor_t motor_;
//static char ref_count_[RecordMsec];
static char actual_count_[RecordTimes];


#if 0
void createRefVelocity(void)
{
  int i;

  for (i = 0; i < RecordMsec; ++i) {
    ref_count_[i] = 10;
  }
}
#endif


static void output(void)
{
  int i;

  for (i = 0; i < RecordTimes; ++i) {
    sci_putint(i);
    sci_puts(", ");

    sci_putint(actual_count_[i]);
    sci_puts("\r\n");

    while (sci_isSending()) {
      ;
    }
  }

  pwm_set(0, PwmMode_Free, 1);

  while (1) {
    ;
  }
}


static void timer_handler(void)
{
  //static int msec = 0;
  static int record_times = 0;

  encoder_update(&encoder_);
  long current_count = encoder_difference(&encoder_);

  enum { RefCount = 10 };
  motor_setVelocity(&motor_, RefCount, current_count);

  //if (++msec >= 10) {
  //msec = 0;
  actual_count_[record_times] = current_count;
  ++record_times;

  if (record_times >= RecordTimes) {
    output();
  }
  //}
}


static void wait(void)
{
  volatile int i;

  for (i = 0; i < 1400000; ++i) {
    ;
  }
}


int main(void)
{
  led_initialize();
  sci_initialize(6, 38400);
  mtu_initialize();
  cmt_initialize(4);
  imask_setMaskLevel(0);

  encoder_initialize(0, &encoder_);
  motor_initialize(0, &motor_);
  motor_set(&motor_, MotorMode_CW, 0);

  // 目標速度の生成
  //createRefVelocity();

  // 制御開始
  cmt_setInterval(10000, timer_handler);
  cmt_start();

  while (1) {
    led_set(0, true);
    led_set(1, false);
    wait();

    led_set(1, true);
    led_set(0, false);
    wait();
  }
}
