//-----------------------------------------------------------
// Embedded coder sample
//-----------------------------------------------------------
#include <stdio.h>
#include <AT91SAM7S.h>
#include <AT91SAM7S_BITDEFINE.h>
#include <vic.h>
#include <piocfg.h>
#include <dx.h>
#include <us.h>

// Simulink Model's header file
#include "WS.h" 
#include "WS.c"

// シリアル通信関連
#define __UDP__
#include "share/us.c"

int cnt=0;

//--------------------------------
//割り込み処理  5msecインターバル
//--------------------------------
void timerint(void){
     cnt++;
     WS_U.tim_c=cnt*0.005;
  }
//========================================================
// main
//========================================================
void main (void) {
  int i,l,c;
  TSyncPosData dat[17];//サーボの数だけ
  int sendData[17];
  enableIRQ ();
  _us_init (_BAUDRATE, txb, sizeof (txb), rxb, sizeof (rxb));
  DX_Init (1000000,11.1,NULL,timerint);

  // Simulink コードの初期化:必ず必要
  WS_initialize(1);
//必要なものは数字を変更する。
  WS_U.side_dat = 3.0;
  WS_U.hohaba_mm = 10.0;
  WS_U.keridashi = 10.0;
  WS_U.speed = 2.0;
  WS_U.hosuu_dat = 1000.0;
  WS_U.koshi_h = 15.0;
  WS_U.leg_hight = 50.0;
  WS_U.up_hight = 50.0;
  WS_U.arm_bend = 10.0;
  WS_U.end_condition = 5.0;
  WS_U.rl_cont = 1.0;
  WS_U.fr_cont = 1.0;
  WS_U.leg_leng = 200.0;
  WS_U.jump = 0.0;
  WS_U.speed_m = 0.5;
  WS_U.shoki_y = 10.0;
 
  DX_wait(3000);
  _us_printf ("START!\n\r");

  while(1){
  
  WS_step();//各関節の角度が10msecごとに計算される。
    for (i = 0; i < 17; i++) {
      sendData[i] =512+3.41* WS_Y.angle_dat[i];
      //ここでは512としているが実際には各サーボの初期値を使用する。
      //3.41は角度をサーボのデータに変換している。AX-12+などはこの数字で良い。
      //サーボの回転軸が異なる場合は+-を軸毎に設定する必要がある。
      //対応軸についてはSimulink Modelをご参照頂きたい。
      dat[i].GoalPosition=sendData[i];
      dat[i].ID=i;
     }
     _us_printf ("\nid=%3d angle=%7d time=%7d",2,sendData[2],(int)WS_Y.hosuu);//ID=2のdataと歩数をprintしている。
   c=17;
   l=DX_SetSyncPosition(dat, c);
   DX_wait(5); // SYNCコマンド送信後に5msのディレイを入れる。
  }
}
