Revision 0:5e4f1e288e2a, committed 2022-01-24

Comitter:

e5119053f6

Date:

Mon Jan 24 03:12:54 2022 +0000

Child:

1:5a65fc20f8c2

Commit message:

ActiveCaster2022_01_24;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AMT22.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,121 @@
+#include "AMT22.h"
+
+AMT203V::AMT203V(SPI* xSPI, PinName xCSBpin){
+  CSBpin = new DigitalOut(xCSBpin);
+  pSPI = xSPI;
+  init_done = false;
+}
+
+// SPI送信部分
+int AMT203V::spi_write(int msg){
+  int msg_temp = 0;
+  CSBpin->write(0);
+  wait_us(3);
+  msg_temp = pSPI->write(msg);
+  CSBpin->write(1);
+  wait_us(3);
+  return(msg_temp);
+}
+
+int AMT203V::init(){
+
+  CSBpin->write(1);
+  pSPI->frequency(2000000);
+
+  int ret = getRawEncount();
+  preABSposition = ABSposition;
+  
+  if(ret == -1) return -1;
+  else init_done = true;
+  
+  return 1;
+}
+
+int AMT203V::getRawEncount(){
+  int recieved;
+  int recieve_count = 0;
+  int error_count = 0;
+  bool recieve_done = false;
+
+  unsigned int  rawValue = 0;
+  bool correct_answer = false;
+
+  CSBpin->write(0);
+  wait_us(3);
+  rawValue |= pSPI->write(0x00) << 8;
+  wait_us(3);
+  rawValue |= pSPI->write(0x00);
+  wait_us(3);
+  CSBpin->write(1);
+  wait_us(3);
+    
+  bool odd, even;
+  for(int i = 0; i < 14; i++){
+      if(i % 2){
+          odd ^= (rawValue >> i) & 0x01;
+      }else{
+          even ^= (rawValue >> i) & 0x01;
+      }
+  }
+  odd = !odd;
+  even = !even;
+
+  correct_answer = (((rawValue >> 15) & 0x01) == odd) && (((rawValue >> 14) & 0x01) == even);
+
+  if(correct_answer){
+      ABSposition = rawValue & 0x3FFF;
+      //printf("ret : %d %x ", ABSposition, rawValue);
+  }/*else{
+      printf("xxx : %d %x\n", rawValue, rawValue);
+  }*/
+  
+  return 1;
+}
+
+int AMT203V::getEncount(){
+  if(init_done){
+    getRawEncount();
+    updateCount();
+    
+    encount = rotation * res + ABSposition;
+    preABSposition = ABSposition;
+
+    //printf("  encount %d abs position %d       ", encount, ABSposition);
+  }
+  else{
+    return -1;
+  }
+  return encount;
+}
+
+// 2,1,0の次に4095ではなくマイナスの値になるように，4093,4094,4095の次に0にならず大きな値になるようにしてる関数
+void AMT203V::updateCount(){
+  if(abs((int)(preABSposition - ABSposition)) >= (int)(res * 0.75)){//3000){
+    if(preABSposition > ABSposition){
+      rotation++;
+    }else if(preABSposition < ABSposition){
+      rotation--;
+    }
+  }
+}
+
+int AMT203V::setZeroPos()
+{
+  int response;
+  int count = 0;
+
+  CSBpin->write(0);
+  wait_us(3);
+  response |= pSPI->write(0x00) << 8;
+  wait_us(3);
+  response |= pSPI->write(0x70);
+  wait_us(3);
+  CSBpin->write(1);
+
+  printf("response %x\n", response);
+
+  getRawEncount();
+  preABSposition = ABSposition;
+
+  return 1;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AMT22.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,34 @@
+#ifndef _AMT22_H
+#define _AMT22_H
+
+#include "mbed.h"
+#include "define.h"
+//#include <SPI.h>
+
+class AMT203V{
+  public:
+  AMT203V(SPI*, PinName);
+
+  int spi_write(int msg);
+  int init();
+  int getEncount();
+  void updateCount();
+  int setZeroPos();
+  int getRawEncount();
+
+  private:
+  bool init_done;
+  //unsigned char CSBpin;
+  const int res = 0x3FFF;//4096;
+  unsigned int ABSposition;  // rawdata
+  unsigned int preABSposition;
+  int rotation = 0;
+  int encount;        // 渡す値
+  int temp[2];
+
+  SPI *pSPI;
+  DigitalOut *CSBpin;
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoControl.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,129 @@
+#include "AutoControl.h"
+
+coords gPosi;
+
+PathTracking motion(FOLLOW_TANGENT); // 経路追従(接線方向向く)モードでとりあえず初期化
+
+AutoControl::AutoControl(){
+}
+
+// SDのデータを読み込んで，PathTrackingの変数に格納
+int AutoControl::init(mySDclass* mySD, int FieldColor){
+    return mySD->path_read(FieldColor, motion.Px, motion.Py, motion.refvel, motion.refangle, motion.acc_mode, motion.acc_count, motion.dec_tbe);
+}
+
+void AutoControl::gPosiInit(){
+    gPosi.x = motion.Px[0];
+    gPosi.y = motion.Py[0];
+    gPosi.z = motion.refangle[0];
+}
+
+coords AutoControl::pathTrackingMode(int mode, int state, int nextPhase){ // 軌道追従モード
+    coords refV;
+    int pathNum = getPathNum();
+
+    if(motion.getMode() != mode) motion.setMode(mode);
+    int syusoku = motion.calcRefvel(); // 収束していれば　1　が返ってくる
+    
+    if(syusoku == 1){ // 収束して次の曲線へ
+        if( pathNum <= state ){
+            motion.Px[3*pathNum+3] = gPosi.x;
+            motion.Py[3*pathNum+3] = gPosi.y;
+            motion.incrPathnum(0.02, 0.997); // 次の曲線へ．括弧の中身は収束に使う数値
+
+            if( pathNum == state ) phase = nextPhase;
+        }
+    }else if(syusoku == 0){ // まだ収束していない，軌道追従中
+        refV.x = motion.refVx;
+        refV.y = motion.refVy;
+        refV.z = motion.refVz;
+    }else{ // それ以外は問題ありなので止める
+        refV.x = 0.0;
+        refV.y = 0.0;
+        refV.z = 0.0;
+    }
+    return refV;
+}
+
+void AutoControl::calibrationGposi(double tempX, double tempY, double tempZ){
+    gPosi.x = tempX;
+    gPosi.y = tempY;
+    gPosi.z = tempZ;
+}
+
+coords AutoControl::commandMode_vel(double tempX, double tempY, double tempZ){
+    coords refV;
+    refV.x = tempX;
+    refV.y = tempY;
+    refV.z = tempZ;
+    return refV;
+}
+
+void AutoControl::commandMode(int nextPhase, bool next/*=true*/){ // 指定した速度で動かすとき
+    int pathNum = getPathNum();
+
+    if( next ){ // この動きを一つの曲線とみなす場合
+        motion.Px[3*pathNum+3] = gPosi.x;
+        motion.Py[3*pathNum+3] = gPosi.y;
+        motion.incrPathnum(0.02, 0.997); // 次の曲線へ．括弧の中身は収束に使う数値
+    }else{
+        motion.Px[3*pathNum] = gPosi.x;
+        motion.Py[3*pathNum] = gPosi.y;
+    }
+    
+    phase = nextPhase;
+}
+
+int AutoControl::getPathNum(){
+    return motion.getPathNum();
+}
+
+double AutoControl::onx(){
+    return motion.onx;
+}
+
+double AutoControl::ony(){
+    return motion.ony;
+}
+
+double AutoControl::angle(){
+    return motion.angle;
+}
+
+double AutoControl::dist(){
+    return motion.dist;
+}
+
+double AutoControl::refKakudo(){
+    return motion.refKakudo;
+}
+
+void AutoControl::initSettings(){
+    motion.initSettings();
+}
+void AutoControl::setConvPara(double conv_length, double conv_tnum){
+    motion.setConvPara(conv_length, conv_tnum);
+}
+void AutoControl::setMaxPathnum(int pathNum){
+    motion.setMaxPathnum(pathNum);
+}
+
+// このメソッドの中身はユーザーが書き換える必要あり
+coords AutoControl::getRefVel(unsigned int swState){
+    coords refV = {0.0, 0.0, 0.0};
+    static unsigned int pre_swState = swState;
+
+    // Edit here >>>>>
+    if(0){
+
+    }
+    else{
+        // とりあえず常に停止するようにしているので，適宜修正してください
+        refV = commandMode_vel(0.0, 0.0, 0.0); // 該当しない場合はとりあえず速度ゼロ
+    }
+    // <<<<<
+
+    pre_swState = swState;
+
+    return refV;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoControl.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,38 @@
+#ifndef AUTOCONTROL_h
+#define AUTOCONTROL_h
+
+#include "PathTracking.h"
+#include "define.h"
+#include "SDclass.h"
+
+class AutoControl{
+    public:
+    AutoControl();
+    int init(mySDclass*, int);
+    void gPosiInit();
+    coords pathTrackingMode(int mode, int state, int nextPhase);
+    void calibrationGposi(double tempX, double tempY, double tempZ);
+    coords commandMode_vel(double tempX, double tempY, double tempZ);
+    void commandMode(int nextPhase, bool next = true);
+    coords getRefVel(unsigned int swState = 0);
+
+    int phase = 0;
+    
+    // mainプログラムとPathTrackingの媒介的な
+    double Px(int);
+    double Py(int);
+    double onx();
+    double ony();
+    double angle();
+    double dist();
+    double refKakudo();
+    void initSettings();
+    void setConvPara(double conv_length, double conv_tnum);
+    void setMaxPathnum(int);
+    int getPathNum();
+
+
+    private:
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,79 @@
+#include "Filter.h"
+
+Filter::Filter(double xint_time)
+{
+    int_time = xint_time;
+    set_t = false;
+}
+
+void Filter::setLowPassPara(double T, double init_data)
+{
+    T_LPF = T;
+    preOutput = init_data;
+    set_t = true;
+}
+
+double Filter::LowPassFilter(double input)
+{
+    //static double preOutput = input;
+    if(!set_t) {
+        return input;
+    } else {
+        double Output = (int_time * input + T_LPF * preOutput)/(T_LPF + int_time);
+        preOutput = Output;
+        return Output;
+    }
+}
+
+void Filter::setSecondOrderPara(double xOmega, double xDzeta, double init_data)
+{
+    omega = xOmega;
+    dzeta = xDzeta;
+    prev_output2 = prev_output1 = init_data;
+
+    set_secorder = true;
+}
+
+void Filter::initPrevData(double init_data)
+{
+    prev_output2 = prev_output1 = init_data;
+}
+
+double Filter::SecondOrderLag(double input)
+{
+    if(!set_secorder) {
+        return input;
+    } else {
+        double output = (2*prev_output1*(1+dzeta*omega*int_time) - prev_output2 + pow(omega, 2.0)*pow(int_time, 2.0)*input)/(1 + 2*dzeta*omega*int_time + pow(omega, 2.0)*pow(int_time, 2.0));
+        prev_output2 = prev_output1;
+        prev_output1 = output;
+        return output;
+    }
+}
+
+void Filter::setNotchPara(double Omega, double int_data)
+{
+    // 落としたい角周波数[rad/s]をOm_nに入れる
+    Om_n = Omega;
+    sq_Om = pow(Om_n, 2.0); // Om_nの2乗
+    sq_dt = pow(int_time, 2.0); // dtの2乗
+
+    n_preOutput[0] = int_data;
+    n_preOutput[1] = int_data;
+
+    n_preInput[0] = int_data;
+    n_preInput[1] = int_data;
+}
+
+double Filter::NotchFilter(double input)
+{
+    double Output = (2*(1 + Om_n * int_time) * n_preOutput[0]-n_preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * n_preInput[0] + n_preInput[1]) / (1 + 2 * Om_n * int_time + sq_Om * sq_dt);
+
+    n_preInput[1] = n_preInput[0];
+    n_preInput[0] = input;
+
+    n_preOutput[1] = n_preOutput[0];
+    n_preOutput[0] = Output;
+
+    return Output;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,41 @@
+#ifndef FILTER_h
+#define FILTER_h
+
+#include "mbed.h"
+
+class Filter
+{
+public:
+    double T_LPF;
+    double Om_n;
+    double sq_dt;
+    double sq_Om;
+    
+    double omega;
+    double dzeta;
+
+    Filter(double);
+    void setLowPassPara(double T, double init_data);
+    double LowPassFilter(double input);
+    
+    void setSecondOrderPara(double xOmega, double xDzeta, double init_data);
+    void initPrevData(double init_data);
+    double SecondOrderLag(double input);
+    
+    void setNotchPara(double Omega, double init_data);
+    double NotchFilter(double input);
+    
+private:
+    double int_time;
+    double preOutput;
+    bool set_t;
+    
+    double prev_output1, prev_output2;
+    bool set_secorder;
+    
+    double n_preOutput[2];
+    double n_preInput[2];
+
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LpmsMe1Peach.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,354 @@
+#include "LpmsMe1Peach.h"
+#include <math.h>
+
+#define PIx2 6.28318530718
+
+// コンストラクタ
+LpmsMe1::LpmsMe1(Serial* xserial){
+  serial = xserial;
+  anglex = 0.0;
+  angley = 0.0;
+  anglez = 0.0;
+  pre_rawanglex = 0.0;
+  pre_rawangley = 0.0;
+  pre_rawanglez = 0.0;
+
+  init_ignore = true;
+  init_done = false;
+}
+
+// コマンドモードへの移行
+void LpmsMe1::goto_command_mode(){
+  serial->putc(0x3A);
+  serial->putc(0x01);//ID
+  serial->putc(0x00);
+  serial->putc(0x06);//command
+  serial->putc(0x00);
+  serial->putc(0x00);
+  serial->putc(0x00);
+  serial->putc(0x07);//check sum
+  serial->putc(0x00);
+  serial->putc(0x0D);
+  serial->putc(0x0A);
+}
+
+// 送信データの変更(角度データのみ)
+void LpmsMe1::set_transmit_data(){
+  serial->putc(0x3A);
+  serial->putc(0x01);//ID
+  serial->putc(0x00);
+  serial->putc(0x0A);//command
+  serial->putc(0x00);
+  serial->putc(0x04);//32bit=4byte送信
+  serial->putc(0x00);
+  serial->putc(0x00);//送信データ１
+  serial->putc(0x00);//送信データ２
+  serial->putc(0x02);//17bit目：角度データのみを取得
+  serial->putc(0x00);//送信データ４
+  serial->putc(0x11);//checksum
+  serial->putc(0x00);
+  serial->putc(0x0D);
+  serial->putc(0x0A);
+}
+
+// フィルタの変更(カルマンフィルタ)
+void LpmsMe1::set_filter_mode(){
+  serial->putc(0x3A);
+  serial->putc(0x01);//ID
+  serial->putc(0x00);
+  serial->putc(0x29);//Command
+  serial->putc(0x00);
+  serial->putc(0x04);//32bit=4byte送信
+  serial->putc(0x00);
+  serial->putc(0x02);//送信データ1 0x02 でカルマンフィルタ
+  serial->putc(0x00);//送信データ2
+  serial->putc(0x00);//送信データ3
+  serial->putc(0x00);//送信データ4
+  serial->putc(0x30);
+  serial->putc(0x00);
+  serial->putc(0x0D);
+  serial->putc(0x0A);
+}
+
+// オフセットの設定
+void LpmsMe1::set_offset(){
+  serial->putc(0x3A);
+  serial->putc(0x01);//ID
+  serial->putc(0x00);
+  serial->putc(0x12);//Command
+  serial->putc(0x00);
+  serial->putc(0x04);//32bit=4byte送信
+  serial->putc(0x00);
+  serial->putc(0x00);//送信データ1 0x00 で object offset
+  serial->putc(0x00);//送信データ2
+  serial->putc(0x00);//送信データ3
+  serial->putc(0x00);//送信データ4
+  serial->putc(0x17);
+  serial->putc(0x00);
+  serial->putc(0x0D);
+  serial->putc(0x0A);
+}
+
+// オフセットのリセット(よくわからないので使ってない)
+void LpmsMe1::reset_orientation_offset(){
+  serial->putc(0x3A);
+  serial->putc(0x01);//ID
+  serial->putc(0x00);
+  serial->putc(0x52);//command
+  serial->putc(0x00);
+  serial->putc(0x00);
+  serial->putc(0x00);
+  serial->putc(0x53);//check sum
+  serial->putc(0x00);
+  serial->putc(0x0D);
+  serial->putc(0x0A);
+}
+
+// センサデータの送信要求
+void LpmsMe1::get_sensor_data(){
+  serial->putc(0x3A);
+  serial->putc(0x01);
+  serial->putc(0x00);
+  serial->putc(0x09);
+  serial->putc(0x00);
+  serial->putc(0x00);
+  serial->putc(0x00);
+  serial->putc(0x0A);//checksum
+  serial->putc(0x00);
+  serial->putc(0x0D);
+  serial->putc(0x0A);
+}
+
+float LpmsMe1::get_z_angle(){
+  if(init_done == true){
+    get_sensor_data();
+    int response = recv_proc(10);
+    // 受信は浮動小数点(符号部，指数部，仮数部)に分かれて送られてくるので，以下でunpacking
+    if(response == 16 ){
+      unsigned int bits_dataz = (buffer[12] | (buffer[13]<<8)) | ((buffer[14]<<16) | (buffer[15]<<24));
+      unsigned int mantissa_part = bits_dataz & 0x7FFFFF; // 仮数部を取得
+      unsigned int copy_matissa_part = mantissa_part;
+      float k = pow(2.0f, -23);
+      float real_mantissa = 0.0f;
+      
+      for(int i = 0; i < 23; i++){
+        real_mantissa += k * (float)(copy_matissa_part & 0x000001); 
+        copy_matissa_part = copy_matissa_part >> 1;
+        k *= 2.0f;
+      }
+      
+      int exponent = (int)((bits_dataz & 0x7F800000) >> 23) - 127; // 指数部を取得
+      float rawanglez = pow(2.0f, exponent) * (1.0f + real_mantissa);
+      if(bits_dataz>>31){ // 符号部の値に応じて+-を変更
+        rawanglez *= -1.0f;
+      }
+
+      // センサは-pi ~ +pi の範囲で取れるので，差分を積算していくことで連続して取れるようにする
+      float diff_rawanglez;
+      if(init_ignore) {
+        diff_rawanglez = 0.0;
+      }else{
+        diff_rawanglez = rawanglez - pre_rawanglez;
+      }
+
+      if(fabs(diff_rawanglez) >= 3.0){ 
+        if(rawanglez < 0){ //+から-へ回ったとき
+          anglez += PIx2 + diff_rawanglez;
+        }else{ // -から+へ回ったとき
+          anglez += -PIx2 + diff_rawanglez;
+        }
+      }else{
+        anglez += diff_rawanglez;
+      }
+      pre_rawanglez = rawanglez;
+
+      return anglez;
+    }
+    return response;
+  }
+  return 0.0; // 初期化が終わっていない場合は，0を返す
+}
+
+int LpmsMe1::recv_proc(int timeout_num = 10){
+  int state = 0, data_counter = 0, timeout_counter = 0;
+  short command = 0, data_length = 0, check_sum = 0, recv_sum = 0;
+  bool recv_done = false;
+  
+  // 受信処理
+  while(!recv_done){
+    if(timeout_counter > timeout_num * 500){
+      return -1;
+    }
+    timeout_counter++;
+    
+    if (serial->readable()) {
+      unsigned char data = serial->getc();
+      //printf("%x ", data);
+
+      switch (state) {
+      case 0:
+        if (data == 0x3A) state++;
+        break;
+      case 1:
+        if (data == 0x01){
+          state++;
+          check_sum += data;
+        }
+        else {
+          state = 0;
+          check_sum = 0;
+        }
+        break;
+      case 2:
+        if (data == 0x00){
+          state++;
+          check_sum += data;
+        }
+        else{
+          state = 0;
+          check_sum = 0;
+        }
+        break;
+      case 3:
+        command |= data;
+        check_sum += data;
+        state++;
+        break;
+      case 4:
+        command |= data<<8;
+        check_sum += data;
+        state++;
+        break;
+      case 5:
+        data_length |= data;
+        check_sum += data;
+        state++;
+        break;
+      case 6:
+        data_length |= data<<8;
+        check_sum += data;
+        if(data_length == 0){
+          state += 2;
+        }else state++;
+        break;
+      case 7:
+        //Serial.print(data, HEX);
+        //Serial.print(" ");
+        buffer[data_counter++] = data;
+        check_sum += data;
+        if (data_counter == data_length) state++;
+        break;
+      case 8:
+        //Serial.println("");
+        recv_sum |= data;
+        state++;
+        break;
+      case 9:
+        recv_sum |= data<<8;
+        if (check_sum == recv_sum) {
+            state++;
+          }else{
+            return -5;
+          }
+        break;
+      case 10:
+        if(data == 0x0D) state++;
+        else return -6;
+        break;
+      case 11:
+        if(data == 0x0A){
+          state++;
+          recv_done = true;
+        }
+        else return -7;
+        break;
+      default:
+        return -8;
+      }
+    }
+  }
+  return data_counter;
+}
+
+int LpmsMe1::init(){
+  unsigned char trash;
+  int datanum, result[4] = {0};
+
+  if(!init_done){
+    // ここは Serial1 をジャイロセンサに使うことを前提に書かれている
+    // P2_3 を CTSピンとして使うための設定
+    /***** ポートの初期化 *****/
+    GPIO.PIBC2 &= ~0x0008; // ポート入力バッファ制御レジスタ 入力バッファ禁止
+    GPIO.PBDC2 &= ~0x0008; // ポート双方向制御レジスタ 双方向モードを禁止
+    GPIO.PM2 &= ~0x0008; // ポートモードレジスタ 入力モード
+    GPIO.PMC2 &= ~0x0008; // ポートモード制御レジスタ ポートモード
+    GPIO.PIPC2 &= ~0x0008; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
+      
+    /***** 入力機能のポート設定 *****/
+    GPIO.PBDC2 |= 0x0008; // ポート双方向制御レジスタ 双方向モードを許可
+      
+    /***** ポート設定 *****/
+    GPIO.PFC2 |= 0x0008;
+    GPIO.PFCE2 &= ~0x0008;
+    GPIO.PFCAE2 |= 0x0008;
+
+    GPIO.PIPC2 |= 0x0008; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
+    GPIO.PMC2 |= 0x0008; // ポートモード制御レジスタ ポートモード
+
+    // CTSピンを使うための設定
+    CPG.STBCR4 &= ~0x40; // FIFO内臓シリアルコミュニケーションインタフェースチャンネル1は動作(これをやらないと以下が書き変わらない)
+    SCIF1.SCFCR |= 0x08; // CTS#1 を利用
+
+    serial->baud(115200);
+
+    //datanum = serial->readable();
+    //for(int i = 0; i < datanum; i++){
+    while(serial->readable()){
+      trash = serial->getc(); // ゴミデータを捨てる
+    }
+    
+    do{
+      goto_command_mode();
+      //Serial.print("goto command mode sent: ");
+      result[0] = recv_proc(500);
+      //Serial.println(result[0]);
+    }while(result[0] != 0);
+    
+    do{
+      set_transmit_data();
+      //Serial.print("set transmit data command sent: ");
+      result[1] = recv_proc(500);
+      //Serial.println(result[1]);
+    }while(result[1] != 0);
+
+    //set_filter_mode();
+    //Serial.print("set filter mode sent: ");
+    //result[2] = recv_proc(1000);
+    //Serial.println(result[2]);
+
+    thread_sleep_for(10); // これがないと，データが正常に取れなくなる
+    
+    do{
+      set_offset();
+      //Serial.print("set offset sent: ");
+      result[3] = recv_proc(500);
+      //Serial.println(result[3]);
+    }while(result[3] != 0);
+
+    for(int i = 0; i < 5; i++){
+      get_z_angle();
+      thread_sleep_for(10);
+    }
+
+    init_ignore = false;
+  }
+
+  //if(result[0] >= 0 && result[1] >= 0) digitalWrite(PIN_LED3, HIGH);
+  //if(result[2] >= 0 && result[3] >= 0) digitalWrite(PIN_LED2, HIGH);
+  if((result[0] + result[1] + result[3]) == 0){
+    init_done = true;
+    return 1;
+  } 
+  else return (result[0] + result[1] + result[3]);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LpmsMe1Peach.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,31 @@
+#ifndef LPMS_ME1_h
+#define LPMS_ME1_h
+
+#include "mbed.h"
+//#include <Arduino.h>
+//#include "RZ_A1H.h"
+
+class LpmsMe1{
+public:
+    LpmsMe1(Serial*);
+    void goto_command_mode();
+    void set_transmit_data();
+    void set_filter_mode();
+    void set_offset();
+    void reset_orientation_offset();
+    void get_sensor_data();
+    float get_z_angle();  
+    int recv_proc(int);
+    int init();
+
+private:
+    Serial* serial;
+    unsigned char buffer[25];
+    float anglex, angley, anglez;
+    float pre_rawanglex, pre_rawangley, pre_rawanglez;
+    bool init_ignore;
+    bool init_done; // 初期化が終わったかどうか
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ManualControl.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,73 @@
+// ゲームコントローラのジョイスティックデータから，ローパスフィルタを用いて指令速度を生成するクラス
+// 
+// 作成日：2019年12月30日
+// 作成者：上野祐樹
+
+#include "ManualControl.h"
+#include "Filter.h"
+
+Filter velX_filter(INT_TIME);
+Filter velY_filter(INT_TIME);
+Filter velZ_filter(INT_TIME);
+
+ManualControl::ManualControl()
+{
+    anglePIDEnable = false;
+}
+
+int ManualControl::init()
+{
+  // コンストラクタでローパスフィルタの初期化ができなかったので修正 
+  velX_filter.setLowPassPara(MANUAL_LOWPASS_T, 0.0);//ローパスフィルタのTと初期値を設定
+  velY_filter.setLowPassPara(MANUAL_LOWPASS_T, 0.0);//ローパスフィルタのTと初期値を設定
+  velZ_filter.setLowPassPara(MANUAL_LOWPASS_T, 0.0);//ローパスフィルタのTと初期値を設定
+  return 0;
+}
+
+coords ManualControl::getRefVel(unsigned int JoyX, unsigned int JoyY, unsigned int JoyZ)
+{
+    int joyraw;
+    coords rawV, refV;
+
+    // ジョイスティックから指令速度を計算する
+    joyraw = (int)JoyX - 127; 
+    if(abs(joyraw) >= JOY_DEADBAND){
+      if(joyraw >= 0){
+        joyraw -= JOY_DEADBAND;
+      }else{
+        joyraw += JOY_DEADBAND;
+      }
+      rawV.x = (joyraw)/(127.0 - (double)JOY_DEADBAND) * JOY_MAXVEL;
+    }else{
+      rawV.x = 0.0;
+    }
+    refV.x = velX_filter.LowPassFilter(rawV.x);
+
+    joyraw = (int)JoyY - 127; 
+    if(abs(joyraw) >= JOY_DEADBAND){
+      if(joyraw >= 0){
+        joyraw -= JOY_DEADBAND;
+      }else{
+        joyraw += JOY_DEADBAND;
+      }
+      rawV.y = (joyraw)/(127.0 - (double)JOY_DEADBAND) * JOY_MAXVEL;
+    }else{
+      rawV.y = 0.0;
+    }
+    refV.y = velY_filter.LowPassFilter(rawV.y);
+
+    joyraw = (int)JoyZ - 127; 
+    if(abs(joyraw) >= JOY_DEADBAND){
+      if(joyraw >= 0){
+        joyraw -= JOY_DEADBAND;
+      }else{
+        joyraw += JOY_DEADBAND;
+      }
+      rawV.z = (joyraw)/(127.0 - (double)JOY_DEADBAND) * JOY_MAXANGVEL;
+    }else{
+      rawV.z = 0.0;
+    }
+    refV.z = velZ_filter.LowPassFilter(rawV.z);
+
+    return refV;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ManualControl.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,21 @@
+#ifndef MANUALCONTROL_h
+#define MANUALCONTROL_h
+
+#include "define.h"
+
+class ManualControl{
+public:
+    /*********** 変数宣言 ***********/
+
+    /*********** 関数宣言 ***********/
+    ManualControl();
+    
+    int init();
+    coords getRefVel(unsigned int, unsigned int, unsigned int);
+
+private:
+    bool anglePIDEnable;
+    
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDclass.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,54 @@
+#include "PIDclass.h"
+
+// コンストラクタPIDパラメータを引数を用いて初期化する
+PID::PID(float xKp, float xKi, float xKd, float xint_time)
+{
+    Kp = xKp;
+    Ki = xKi;
+    Kd = xKd;
+    int_time = xint_time;
+
+    preError = 0.0; // 1個前のエラーの値
+    intError = 0.0; // 積分値の初期化
+
+    init_done = false;
+}
+
+void PID::PIDinit(float ref, float act)
+{
+    preError = ref - act;
+    intError = 0.0; // 積分値の初期化
+
+    init_done = true;
+}
+
+// PID制御の実体部
+float PID::getCmd(float ref, float act, float maxcmd)
+{
+    float cmd, Error, dError;
+    cmd = 0.0;
+
+    if(init_done) {
+        Error = ref - act;
+        cmd += Error * Kp;
+
+        dError = (Error - preError);// / int_time; int_timeが0.01のときdErrorの値が大きくなりすぎてしまうのでコメントアウト
+        cmd += dError * Kd;
+
+        intError += (Error + preError) / 2 * int_time;
+        cmd += intError * Ki;
+
+        preError = Error;
+
+        if(cmd > maxcmd) cmd = maxcmd;
+        if(cmd < -maxcmd) cmd = -maxcmd;
+    }
+    return cmd;
+}
+
+void PID::setPara(float xKp, float xKi, float xKd)
+{
+    Kp = xKp;
+    Ki = xKi;
+    Kd = xKd;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDclass.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,26 @@
+#ifndef PIDCLASS_h
+#define PIDCLASS_h
+
+#include "mbed.h"
+
+class PID
+{
+public:
+    PID(float xKp, float xKi, float xKd, float xint_time);
+    float getCmd(float ref, float act, float maxcmd);
+    void PIDinit(float ref, float act);
+    void setPara(float xKp, float xKi, float xKd);
+
+private:
+    float preError;
+    float intError;
+    float Kp;
+    float Ki;
+    float Kd;
+    float int_time;
+    
+    bool init_done;
+    
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PathTracking.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,327 @@
+//-----------------------------------------
+// 軌道追従や位置のPID制御を行うためのクラス
+// 作成：2019/05/15 by Yuki Ueno
+// 編集：Miki Nakaone
+//-----------------------------------------
+
+#include "PathTracking.h"
+
+extern coords gPosi;
+
+PID posiPIDx(POSI_X_KP, POSI_X_KI, POSI_X_KD, INT_TIME);
+PID posiPIDy(POSI_Y_KP, POSI_Y_KI, POSI_Y_KD, INT_TIME);
+PID posiPIDz(POSI_Z_KP, POSI_Z_KI, POSI_Z_KD, INT_TIME);
+
+PID yokozurePID(YOKOZURE_KP, YOKOZURE_KI, YOKOZURE_KD, INT_TIME);//(3.0, 0.0, 0.0, INT_TIME);
+PID kakudoPID(KAKUDO_KP, KAKUDO_KI, KAKUDO_KD, INT_TIME);
+
+// 二次遅れ使えるようになる
+Filter sokduo_filter(INT_TIME);
+Filter kakudo_filter(INT_TIME);
+
+// コンストラクタ
+PathTracking::PathTracking(int xmode){
+    path_num = 0;
+    max_pathnum = 0;
+    t_be = 0.0;
+    pre_t_be = 0.1;
+    epsilon = 1.0;
+
+    refVx = 0;
+    refVy = 0;
+    refVz = 0;
+
+    count_acc = 0;
+
+    mode = xmode;
+
+    mode_changed = true;
+    init_done = false;
+}
+
+// tを求めるための方程式
+double PathTracking::func(int p, double t)
+{
+    return a_be[p] * pow(t, 5.0) + b_be[p] * pow(t,4.0) + c_be[p] * pow(t,3.0) + d_be[p] * pow(t,2.0) + e_be[p] * t + f_be[p];
+}
+// tを求めるための方程式の1階微分
+double PathTracking::dfunc(int p, double t)
+{
+    return 5.0 * a_be[p] * pow(t, 4.0) +  4.0 * b_be[p] * pow(t,3.0) + 3.0 * c_be[p] * pow(t,2.0) + 2.0 * d_be[p] * t + e_be[p];
+}
+
+// tにおけるベジエ曲線の座標を求める関数
+double PathTracking::bezier_x(int p, double t)
+{
+    return Ax[p]*pow(t,3.0) + 3.0*Bx[p]*pow(t,2.0) + 3.0*Cx[p]*t + Dx[p];
+}
+double PathTracking::bezier_y(int p, double t)
+{
+    return Ay[p]*pow(t,3.0) + 3.0*By[p]*pow(t,2.0) + 3.0*Cy[p]*t + Dy[p];
+}
+
+// ベジエ曲線式の1階微分
+double PathTracking::dbezier_x(int p, double t)
+{
+    return 3.0*Ax[p]*pow(t,2.0) + 6.0*Bx[p]*t + 3.0*Cx[p];
+}
+double PathTracking::dbezier_y(int p, double t)
+{
+    return 3.0*Ay[p]*pow(t,2.0) + 6.0*By[p]*t + 3.0*Cy[p];
+}
+
+// ニュートン法のための係数の初期化
+void PathTracking::initSettings(){
+    // PID関連初期化
+	posiPIDx.PIDinit(0.0, 0.0);
+	posiPIDy.PIDinit(0.0, 0.0);
+	posiPIDz.PIDinit(0.0, 0.0);
+	
+	yokozurePID.PIDinit(0.0, 0.0);
+	kakudoPID.PIDinit(0.0, 0.0);
+    
+    sokduo_filter.setSecondOrderPara(FILT_SOKUDO_OMEGA, FILT_SOKUDO_DZETA, 0.0);//(15.0, 1.0, 0.0);
+    kakudo_filter.setSecondOrderPara(FILT_KAKUDO_OMEGA, FILT_KAKUDO_DZETA, 0.0);//(7.0, 1.0, 0.0);
+    
+    for(int i = 0; i < PATHNUM; i++) {
+        Ax[i] = Px[3*i+3] -3*Px[3*i+2] + 3*Px[3*i+1] - Px[3*i+0];
+        Ay[i] = Py[3*i+3] -3*Py[3*i+2] + 3*Py[3*i+1] - Py[3*i+0];
+        Bx[i] = Px[3*i+2] -2*Px[3*i+1] + Px[3*i+0];
+        By[i] = Py[3*i+2] -2*Py[3*i+1] + Py[3*i+0];
+        Cx[i] = Px[3*i+1] - Px[3*i+0];
+        Cy[i] = Py[3*i+1] - Py[3*i+0];
+        Dx[i] = Px[3*i+0];
+        Dy[i] = Py[3*i+0];
+    }
+
+    for(int i = 0; i < PATHNUM; i++) {
+        a_be[i] = pow(Ax[i], 2.0) + pow(Ay[i], 2.0);
+        b_be[i] = 5*(Ax[i]*Bx[i] + Ay[i]*By[i]);
+        c_be[i] = 2*((3*pow(Bx[i],2.0)+2*Ax[i]*Cx[i]) + (3*pow(By[i],2.0)+2*Ay[i]*Cy[i]));
+        d_be_[i] = 9*Bx[i]*Cx[i] + 9*By[i]*Cy[i];
+        e_be_[i] = 3*pow(Cx[i],2.0) + 3*pow(Cy[i],2.0);
+        f_be_[i] = 0;
+    }
+    init_done = true;
+}
+
+// ベジエ曲線までの垂線距離をニュートン法で求めて，そこまでの距離と接線角度を計算する
+void PathTracking::calcRefpoint(){
+    if(init_done){
+        double tmpx = Px[path_num * 3] - gPosi.x;
+        double tmpy = Py[path_num * 3] - gPosi.y;
+                
+        d_be[path_num] = d_be_[path_num] + Ax[path_num] * tmpx + Ay[path_num] * tmpy;
+        e_be[path_num] = e_be_[path_num] + 2*Bx[path_num] * tmpx + 2*By[path_num] * tmpy;
+        f_be[path_num] = f_be_[path_num] + Cx[path_num] * tmpx + Cy[path_num] * tmpy;
+                
+        int count_newton = 0;
+        do {
+            t_be = pre_t_be - func(path_num, pre_t_be)/dfunc(path_num, pre_t_be);
+            epsilon = fabs((t_be - pre_t_be)/pre_t_be);
+            
+            pre_t_be = t_be;
+            count_newton++;
+        }while(epsilon >= 1e-4 && count_newton <= 50);
+        
+        //double onx, ony;    //ベジエ曲線上の点
+        onx = bezier_x(path_num, t_be);
+        ony = bezier_y(path_num, t_be);
+                
+        // 外積による距離導出
+        //double angle;
+        angle = atan2(dbezier_y(path_num, t_be), dbezier_x(path_num, t_be)); // ベジエ曲線の接線方向
+        if(fabs(angle - preAngle) > M_PI){
+            angle += 2 * M_PI;
+        }
+        //double dist;
+        dist = (ony - gPosi.y)*cos(angle) - (onx - gPosi.x)*sin(angle);
+
+        epsilon = 1.0;
+    }
+}
+
+// モードによって，それぞれ指令速度を計算する
+int PathTracking::calcRefvel(){
+    double refVxg, refVyg, refVzg; // グローバル座標系の指定速度
+    double tmpPx, tmpPy;
+    //static int counter = 0;
+
+    if(init_done){
+        if(path_num <= max_pathnum){ // パスが存在する場合は以下の処理を行う
+            if(mode == FOLLOW_TANGENT || mode == FOLLOW_COMMAND){ // ベジエ曲線追従モード
+                calcRefpoint();
+
+                double refVtan, refVper, refVrot;
+                if((acc_mode[path_num] == MODE_START || acc_mode[path_num] == MODE_START_STOP) && count_acc <= acc_count[path_num]){
+                    count_acc++;
+                    refVtan = refvel[path_num] * count_acc/(double)acc_count[path_num];
+                }else if(t_be < dec_tbe[path_num]){
+                    refVtan = refvel[path_num];
+                }else if((acc_mode[path_num] == MODE_STOP || acc_mode[path_num] == MODE_START_STOP) && t_be >= dec_tbe[path_num]){
+                    refVtan = refvel[path_num] - refvel[path_num] * (t_be - dec_tbe[path_num]) / (1.0 - dec_tbe[path_num]);
+                }else if(t_be >= dec_tbe[path_num]){
+                    refVtan = refvel[path_num] - (refvel[path_num] - refvel[path_num + 1]) * (t_be - dec_tbe[path_num]) / (1.0 - dec_tbe[path_num]);
+                }
+
+                //refVtan = sokduo_filter.SecondOrderLag(refvel[path_num]); // 接線方向速度
+                refVper = yokozurePID.getCmd(dist, 0.0, refvel[path_num] * 2.0); // 横方向速度
+
+                // 旋回は以下の2種類を mode によって変える
+                if(mode == FOLLOW_TANGENT){
+                    if(mode_changed){
+                        kakudoPID.PIDinit(angle, gPosi.z);
+                        mode_changed = false;
+                    }
+                    refVrot = kakudoPID.getCmd(angle, gPosi.z, 1.57);//(refKakudo, gPosiz, 1.57);
+                }else{
+                    if(mode_changed){
+                        kakudoPID.PIDinit(refKakudo, gPosi.z);
+                        kakudo_filter.initPrevData(refKakudo);
+                        mode_changed = false;
+                    }
+                    refKakudo = kakudo_filter.SecondOrderLag(refangle[path_num]);
+                    refVrot = kakudoPID.getCmd(refKakudo, gPosi.z, 1.57);
+                }
+
+                per = refVper;
+                rot = refVrot;
+                // ローカル座標系の指令速度(グローバル座標系のも込み込み)
+                //refVxとrefVyをコメントアウトするとkakudoPIDのパラメータ調整が出来る(手で押してみて…)
+                //refVperだけにするとyokozurePIDのパラメータ調整できる
+                refVx =  refVtan * cos( gPosi.z - angle ) + refVper * sin( gPosi.z - angle );
+                refVy = -refVtan * sin( gPosi.z - angle ) + refVper * cos( gPosi.z - angle );
+                refVz =  refVrot;
+            }else{ // PID位置制御モード
+                if( mode_changed ){
+                    Px[3 * path_num] = gPosi.x;
+                    Py[3 * path_num] = gPosi.y;
+                    posiPIDx.PIDinit(Px[3 * path_num], gPosi.x);	// ref, act
+                    posiPIDy.PIDinit(Py[3 * path_num], gPosi.y);
+                    posiPIDz.PIDinit(refKakudo, gPosi.z);
+                    kakudo_filter.initPrevData(refKakudo);
+                    setRefKakudo();
+                    mode_changed = false;
+                }
+
+                if(count_acc <= acc_count[path_num]){
+                    count_acc++;
+                    tmpPx = Px[3 * path_num] + (Px[3 * path_num + 3] - Px[3 * path_num]) * count_acc / (double)acc_count[path_num];
+                    tmpPy = Py[3 * path_num] + (Py[3 * path_num + 3] - Py[3 * path_num]) * count_acc / (double)acc_count[path_num];
+                }else{
+                    tmpPx = (Px[3 * path_num + 3]);
+                    tmpPy = (Py[3 * path_num + 3]);
+                }
+
+                // PIDクラスを使って位置制御を行う(速度の指令値を得る)
+                refVxg = posiPIDx.getCmd(tmpPx, gPosi.x, refvel[path_num]);//(Px[30], gPosix, refvel[phase]);
+                refVyg = posiPIDy.getCmd(tmpPy, gPosi.y, refvel[path_num]);//(Py[30], gPosiy, refvel[phase]);
+                refKakudo = kakudo_filter.SecondOrderLag(refangle[path_num]);
+                refVzg = posiPIDz.getCmd(refKakudo, gPosi.z, 1.57);//角速度に対してrefvelは遅すぎるから　refvel[path_num]);//(0.0, gPosiz, refvel[phase]);
+
+                // 上記はグローバル座標系における速度のため，ローカルに変換
+                refVx =  refVxg * cos(gPosi.z) + refVyg * sin(gPosi.z);
+                refVy = -refVxg * sin(gPosi.z) + refVyg * cos(gPosi.z);
+                refVz =  refVzg;
+            }
+
+            // 収束判定して，収束していたら　1　を返す
+            dist2goal = sqrt(pow(gPosi.x - Px[3 * path_num + 3], 2.0) + pow(gPosi.y - Py[3 * path_num + 3], 2.0));
+            if(mode == FOLLOW_TANGENT || mode == FOLLOW_COMMAND){
+                // 軌道追従制御なら，到達位置からの距離とベジエ曲線の t のどちらかの条件
+                if(dist2goal <= conv_length || t_be >= conv_tnum){
+                    //counter = 0;
+                    return 1;
+                }
+            }if(mode == POSITION_PID){
+                // 位置制御なら，目標位置と角度両方を見る
+                if(dist2goal <= conv_length && fabs(refangle[path_num] - gPosi.z)){
+                    //counter = 0;
+                    return 1;
+                }
+            }
+            
+            // 収束していなかったら　0　を返す
+            return 0;
+        }else{
+            // path_num が設定された max_pathnum を超えたら　-2　を返す
+            return -2;
+        }
+    
+    }else{
+        // 初期化されていなかったら　-1　を返す
+        return -1;
+    }
+}
+
+// ベジエ曲線のパス番号をインクリメントする
+void PathTracking::incrPathnum(double xconv_length, double xconv_tnum = 0.997){
+    path_num++;
+    pre_t_be = 0.1;
+    count_acc = 0;
+    setConvPara(xconv_length, xconv_tnum);
+}
+
+int PathTracking::getPathNum(){
+    return path_num;
+}
+
+void PathTracking::setPathNum(int num){
+    path_num = num;
+}
+
+// 収束判定に用いる距離などをセットする
+void PathTracking::setConvPara(double xconv_length, double xconv_tnum = 0.997){
+    conv_length = xconv_length;
+    conv_tnum = xconv_tnum;
+}
+
+void PathTracking::setMode(int xmode){
+    mode = xmode;
+    mode_changed = true;
+}
+
+int PathTracking::getMode(){
+    return mode;
+}
+
+void PathTracking::setMaxPathnum(int num){
+    max_pathnum = num;
+}
+
+void PathTracking::setPosiPIDxPara(float xKp, float xKi, float xKd){
+    posiPIDx.setPara(xKp, xKi, xKd);
+}
+
+void PathTracking::setPosiPIDyPara(float xKp, float xKi, float xKd){
+    posiPIDy.setPara(xKp, xKi, xKd);
+}
+
+void PathTracking::setPosiPIDzPara(float xKp, float xKi, float xKd){
+    posiPIDz.setPara(xKp, xKi, xKd);
+}
+
+void PathTracking::setYokozurePIDPara(float xKp, float xKi, float xKd){
+    yokozurePID.setPara(xKp, xKi, xKd);
+}
+
+void PathTracking::setKakudoPIDPara(float xKp, float xKi, float xKd){
+    kakudoPID.setPara(xKp, xKi, xKd);
+}
+
+
+void PathTracking::kakudoPIDinit(){
+    kakudoPID.PIDinit(refKakudo, gPosi.z);
+}
+
+void PathTracking::setRefKakudo(){
+    refKakudo = refangle[path_num];
+}
+
+double PathTracking::getRefVper(){
+    return per;
+}
+
+double PathTracking::getRefVrot(){
+    return rot;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PathTracking.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,116 @@
+#ifndef PATHTRACKING_h
+#define PATHTRACKING_h
+
+#include "mbed.h"
+#include "PIDclass.h"
+#include "Filter.h"
+#include "define.h"
+
+#define PATHNUM 50
+#define POINTNUM 100
+
+#define FOLLOW_TANGENT  ( 0 )
+#define FOLLOW_COMMAND  ( 1 )
+//#define FOLLOW_ICHIPID  ( 2 )
+#define POSITION_PID    ( 2 )
+
+#define MODE_START  ( 0 )
+#define MODE_NORMAL ( 1 )
+#define MODE_STOP   ( 2 )
+#define MODE_START_STOP   ( 3 )
+
+class PathTracking{
+public:
+    /*********** 変数宣言 ***********/
+    double Px[POINTNUM], Py[POINTNUM];
+    double refangle[PATHNUM], refvel[PATHNUM];
+    int acc_mode[PATHNUM], acc_count[PATHNUM];
+    double dec_tbe[PATHNUM];
+
+    // ベジエ曲線関連
+    double Ax[ PATHNUM ];
+    double Bx[ PATHNUM ];
+    double Cx[ PATHNUM ];
+    double Dx[ PATHNUM ];
+
+    double Ay[ PATHNUM ];
+    double By[ PATHNUM ];
+    double Cy[ PATHNUM ];
+    double Dy[ PATHNUM ];
+
+    // 内積関連
+    double a_be[ PATHNUM ];
+    double b_be[ PATHNUM ];
+    double c_be[ PATHNUM ];
+    double d_be[ PATHNUM ];
+    double e_be[ PATHNUM ];
+    double f_be[ PATHNUM ];
+    double d_be_[ PATHNUM ];
+    double e_be_[ PATHNUM ];
+    double f_be_[ PATHNUM ];
+
+    double onx, ony;
+    double angle, dist;
+    double preAngle = 1.5708;
+    double t_be, pre_t_be;
+    double dist2goal;
+    double epsilon;
+
+    double refVx, refVy, refVz;
+    double refKakudo;
+    double tmpPx, tmpPy;
+
+    /*********** 関数宣言 ***********/
+    PathTracking(int xmode);
+    // tを求めるための方程式
+    double func(int p, double t);
+    double dfunc(int p, double t);
+    // tにおけるベジエ曲線の座標を求める関数
+    double bezier_x(int p, double t);
+    double bezier_y(int p, double t);
+    // ベジエ曲線式の1階微分
+    double dbezier_x(int p, double t);
+    double dbezier_y(int p, double t);
+
+    void initSettings();
+
+    void calcRefpoint();
+    int calcRefvel();
+    
+    void incrPathnum(double conv_length, double conv_tnum);
+    void setConvPara(double conv_length, double conv_tnum);
+    int getPathNum();
+    void setPathNum(int);
+
+    void setMode(int);
+    int getMode();
+
+    void setMaxPathnum(int);
+
+    void setPosiPIDxPara(float xKp, float xKi, float xKd);
+    void setPosiPIDyPara(float xKp, float xKi, float xKd);
+    void setPosiPIDzPara(float xKp, float xKi, float xKd);
+    void setYokozurePIDPara(float xKp, float xKi, float xKd);
+    void setKakudoPIDPara(float xKp, float xKi, float xKd);
+    void kakudoPIDinit();
+    void setRefKakudo();
+
+    double getRefVper();
+    double getRefVrot();
+
+private:
+    int path_num;
+    int mode;
+    int max_pathnum;
+    int count_acc;
+
+    double conv_length;
+    double conv_tnum;
+
+    bool mode_changed;
+    bool init_done;
+
+    double tan, per, rot;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Platform.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,198 @@
+// 4輪オムニやメカナム，双輪キャスタなどのプラットフォームごとに
+// 各モータの指令速度の計算からRoboClawの制御，自己位置推定を行なうクラス
+// Platform.h の DRIVE_UNIT の定義でプラットフォームを選択する
+// 作成日：2019年12月30日
+// 作成者：上野祐樹
+
+//アクティブキャスタの処理を追加
+//追加日:2021年12月22日
+//編集者:小林亮紘
+
+#include "mbed.h"
+#include "Platform.h"
+//#include "AMT22.h"
+//#include "RoboClaw.h"
+//#include "AMT203VPeach.h"
+
+//RoboClaw MD(&SERIAL_ROBOCLAW,1);
+
+#if DRIVE_UNIT == PLATFORM_DUALWHEEL
+    AMT203V amt203(&SPI, PIN_CSB);
+#elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
+	//SPI spi(P10_14, P10_15, P10_12); // mosi, miso, sclk
+	//AMT203V absenc_1(&spi, PIN_CSB_1);
+	//AMT203V absenc_2(&spi, PIN_CSB_2);
+	//AMT203V absenc_3(&spi, PIN_CSB_3);
+#endif
+
+Platform::Platform(int dir1, int dir2, int dir3, int dir4){
+    rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
+    rotateDir[1] = (dir2 > 0) - (dir2 < 0);
+    rotateDir[2] = (dir3 > 0) - (dir3 < 0);
+    rotateDir[3] = (dir4 > 0) - (dir4 < 0);
+
+    init_done = false;
+}
+
+Platform::Platform(int dir1, int dir2, int dir3){
+    rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
+    rotateDir[1] = (dir2 > 0) - (dir2 < 0);
+    rotateDir[2] = (dir3 > 0) - (dir3 < 0);
+    rotateDir[3] = 1;
+    init_done = false;
+}
+
+Platform::Platform(){
+    rotateDir[0] = 1; // 指示がないときはすべて1
+    rotateDir[1] = 1;
+    rotateDir[2] = 1;
+    rotateDir[3] = 1;
+
+    init_done = false;
+}
+
+// 自己位置推定の初期化
+void Platform::platformInit(coords initPosi){
+#if DRIVE_UNIT == PLATFORM_DUALWHEEL
+    SPI.begin(); // ここでSPIをbeginしてあげないとちゃんと動かなかった
+    SPI.setClockDivider(SPI_CLOCK_DIV16); //SPI通信のクロックを1MHzに設定 beginの後に置かないと，処理が止まる
+    stateamt203 = amt203.init();
+#elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
+	//printf("AMT222 initialization %d %d %d\r\n", absenc_1.init(), absenc_2.init(), absenc_3.init());
+#endif
+
+    //MD.begin(115200);
+
+    Posi = initPosi;
+
+    preEncX = 0;
+    preEncY = 0;
+    pre_angle_rad = 0.0;//Posi.z;
+    init_done = true;
+}
+
+// 自己位置推定値(Posi)を外部からセット
+void Platform::setPosi(coords tempPosi){
+    Posi = tempPosi;
+}
+
+// エンコーダのカウント値と，ジャイロセンサから取得した角度をもとに自己位置を計算する
+coords Platform::getPosi(int encX, int encY, double angle_rad){
+    if(init_done){
+        // ローカル座標系での変化量を計算(zは角度)
+        coords diff;
+
+        // エンコーダのカウント値から角度の変化量を計算する
+        double angX, angY;
+        angX = (double)( encX - preEncX ) * _2PI_RES4;
+        angY = (double)( encY - preEncY ) * _2PI_RES4;
+        
+        double angle_diff;
+        angle_diff = angle_rad - pre_angle_rad; // 角度の変化量を計算
+        diff.z = angle_diff;
+        diff.x = RADIUS_X * angX; //RADIUS_X はX軸エンコーダの車輪半径
+        diff.y = RADIUS_Y * angY; //RADIUS_Y はY軸エンコーダの車輪半径
+
+        // グローバル座標系での変化量に変換し，これまでのデータに加算することで自己位置推定完了
+        Posi.z += diff.z;
+        Posi.x += diff.x * cos( Posi.z ) - diff.y * sin( Posi.z );
+        Posi.y += diff.x * sin( Posi.z ) + diff.y * cos( Posi.z );
+        
+        // 1サンプル前のデータとして今回取得したデータを格納
+        preEncX = encX;
+        preEncY = encY;
+        pre_angle_rad = angle_rad;
+    }
+    return Posi;
+}
+
+void Platform::VelocityControl(coords refV){
+    if(init_done){
+        #if DRIVE_UNIT == PLATFORM_OMNI3WHEEL
+            double refOmegaA, refOmegaB, refOmegaC;
+            // 車輪反時計方向が正
+            refOmegaA = (-refV.y - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+            refOmegaB = ( refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+            refOmegaC = (-refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+
+            // RoboClawの指令値に変換
+            double mdCmdA, mdCmdB, mdCmdC;
+            mdCmdA = refOmegaA * _2RES_PI;
+            mdCmdB = refOmegaB * _2RES_PI;
+            mdCmdC = refOmegaC * _2RES_PI;
+
+            // モータにcmdを送り，回す
+            //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]);// 右前
+            //MD.SpeedM2(ADR_MD1, (int)mdCmdB * rotateDir[1]);// 左前
+            //MD.SpeedM2(ADR_MD2, (int)mdCmdC * rotateDir[2]);// 右後
+         #elif DRIVE_UNIT == PLATFORM_OMNI4WHEEL
+            double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
+            // 車輪反時計方向が正
+            refOmegaA = ( refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO; // [rad/s]
+            refOmegaB = ( refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+            refOmegaC = (-refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+            refOmegaD = (-refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+
+            // RoboClawの指令値に変換
+            double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
+            mdCmdA = refOmegaA * _2RES_PI;
+            mdCmdB = refOmegaB * _2RES_PI;
+            mdCmdC = refOmegaC * _2RES_PI;
+            mdCmdD = refOmegaD * _2RES_PI;
+
+            //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]); // 左前 ①
+            //MD.SpeedM2(ADR_MD1, (int)mdCmdC * rotateDir[1]); // 左後 ②
+            //MD.SpeedM1(ADR_MD2, (int)mdCmdD * rotateDir[2]); // 右後 ③
+            //MD.SpeedM2(ADR_MD2, (int)mdCmdB * rotateDir[3]); // 右前 ④
+        #elif DRIVE_UNIT == PLATFORM_DUALWHEEL
+            // ターンテーブルの角度取得
+            double thetaDuEnc, thetaDu;
+            static double  preThetaDuEnc = thetaDuEnc;
+            thetaDuEnc = amt203.getEncount(); 
+            if( thetaDuEnc == -1 ){
+            thetaDuEnc = preThetaDuEnc; // -1はエラーなので，前の値を格納しておく
+            }
+            preThetaDuEnc = thetaDuEnc;
+            thetaDu = (double)thetaDuEnc*2*PI / TT_RES4;	// 角度に変換
+            
+            // 車輪やターンテーブルの指令速度を計算
+            double cosDu, sinDu, refOmegaR, refOmegaL, refOmegaT;
+            cosDu = cos(thetaDu);
+            sinDu = sin(thetaDu);
+            refOmegaR = ( ( cosDu - sinDu ) * refV.x + ( sinDu + cosDu ) * refV.y ) / RADIUS_R;// right
+            refOmegaL = ( ( cosDu + sinDu ) * refV.x + ( sinDu - cosDu ) * refV.y ) / RADIUS_L;// left
+            refOmegaT = ( - ( 2 * sinDu / W ) * refV.x + ( 2 * cosDu / W ) * refV.y - refV.z ) * GEARRATIO;// turntable
+
+            // RoboClawの指令値に変換
+            double mdCmdR, mdCmdL, mdCmdT;
+            mdCmdR = refOmegaR * _2RES_PI;
+            mdCmdL = refOmegaL * _2RES_PI;
+            mdCmdT = refOmegaT * _2RES_PI_T;
+
+            // モータにcmdを送り，回す
+            //MD.SpeedM1(ADR_MD1, -(int)mdCmdR);// 右車輪
+            //MD.SpeedM2(ADR_MD1,  (int)mdCmdL);// 左車輪
+            //MD.SpeedM1(ADR_MD2,  (int)mdCmdT);// ターンテーブル
+        #elif DRIVE_UNIT == PLATFORM_MECHANUM
+            double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
+            // 車輪の軸まわりで見て，反時計方向が正
+            refOmegaA = ( refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左前
+            refOmegaB = ( refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左後
+            refOmegaC = (-refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右後
+            refOmegaD = (-refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右前
+
+            // RoboClawの指令値に変換
+            double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
+            mdCmdA = refOmegaA * _2RES_PI;
+            mdCmdB = refOmegaB * _2RES_PI;
+            mdCmdC = refOmegaC * _2RES_PI;
+            mdCmdD = refOmegaD * _2RES_PI;
+
+            //MD.SpeedM1(ADR_MD1,  (int)mdCmdA * rotateDir[0]);// 左前
+            //MD.SpeedM2(ADR_MD1,  (int)mdCmdB * rotateDir[1]);// 左後
+            //MD.SpeedM1(ADR_MD2,  (int)mdCmdC * rotateDir[2]);// 右後
+            //MD.SpeedM2(ADR_MD2,  (int)mdCmdD * rotateDir[3]);// 右前
+
+        #endif
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Platform.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,27 @@
+#ifndef PLATFORM_h
+#define PLATFORM_h
+
+#include "define.h"
+
+class Platform{
+public:
+    /*********** 変数宣言 ***********/
+
+    /*********** 関数宣言 ***********/
+    Platform(int dir1 = 1, int dir2 = 1, int dir3 = 1, int dir4 = 1);
+    Platform(int dir1 = 1, int dir2 = 1, int dir3 = 1);
+    Platform();
+    void platformInit(coords);
+    void setPosi(coords);
+    coords getPosi(int, int, double);
+    void VelocityControl(coords);
+
+private:
+    coords Posi;
+    int preEncX, preEncY;
+    double pre_angle_rad;
+    bool init_done;
+    int rotateDir[4];
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDclass.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,283 @@
+#include "SDclass.h"
+
+char REDpath[20] = "/fs/PATH_RED.txt";
+char REDvel[20] = "/fs/VEL_RED.txt";
+char BLUpath[20] = "/fs/PATH_BLU.txt";
+char BLUvel[20] = "/fs/VEL_BLU.txt";
+
+FATFileSystem fs("fs");
+SDBlockDevice sd(P8_5, P8_6, P8_3, P8_4);
+
+mySDclass::mySDclass() {}
+
+/* SDカードの初期化 */
+int mySDclass::init() {
+  printf("mySDclass");
+
+  if (fs.mount(&sd) == 0) {
+    sd.frequency(2000000);
+    SD_enable = true;
+    printf(" init done");
+    return 0;
+  }
+
+  printf(" init failure");
+  return -1;
+}
+
+/* ログデータ書き出し用ファイル名の設定 */
+int mySDclass::make_logfile() {
+  // bool nameOK = false;
+  int file_num = 0;
+
+  DIR *d;
+  d = opendir("/fs");
+  if (d != NULL) {
+    while (readdir(d) != NULL) {
+      file_num++;
+    }
+  }
+
+  int keta = 0;
+  for(int tmp = file_num; (tmp /= 10) != 0; keta++);
+  logFileName = "/fs/LOG_";
+  for(int i = 0; i < (2 - keta); i++){
+      logFileName += "0";
+  }
+  logFileName += to_string(file_num);
+  logFileName += ".txt";
+  //c_logFileName = logFileName.c_str();
+  
+  return file_num;
+}
+
+/* ログデータ書き出し用の関数 */
+int mySDclass::write_logdata(string dataString) {
+  FILE *dataFile = fopen(logFileName.c_str(), "w");
+
+  if (dataFile) {
+    fprintf(dataFile, dataString.c_str());
+    fclose(dataFile);
+    // Serial.println(dataString);
+    return 0;
+  } else {
+    // Serial.println("error opening ");
+    return -1;
+  }
+}
+
+int mySDclass::path_read(int field, double Px[], double Py[], double vel[],
+                         double angle[], int mode[], int count[],
+                         double tbe[]) {
+  FILE *myFile;
+  char *pathFile;
+  char *velFile;
+  char tmpchar;
+  char tmpA[10], tmpB[10], tmpC[10], tmpD[10], tmpE[10];
+  bool file_end = false;
+  int numa = 0, numb = 0, numc = 0, numd = 0, nume = 0;
+  int path_num = 0, point_num = 0;
+
+  // 赤か青かで読み込むファイルを変更する
+  if (field == RED) {
+    pathFile = REDpath; // 赤ゾーンのパス設定ファイル
+    velFile = REDvel;   // 赤ゾーンの速度と角度の設定ファイル
+  } else if (field == BLUE) {
+    pathFile = BLUpath; // 青ゾーンのパス設定ファイル
+    velFile = BLUvel;   // 青ゾーンの速度と角度の設定ファイル
+  } else {
+    return -1;
+  }
+
+  // パス設定用ファイルからデータを読み込む
+  printf("openning... %s\n", pathFile);
+  myFile = fopen(pathFile, "r");
+
+  if (myFile) {
+    // read from the file until there's nothing else in it:
+    while (!file_end && !feof(myFile)) {
+      while ((tmpchar = fgetc(myFile)) != ',') { // カンマが来るまで繰り返し
+        tmpA[numa] = tmpchar; // 文字列に1文字ずつ格納していく
+        numa++;
+      }
+      *Px = str2double(tmpA, numa); //関数でdoubleに変換
+      // Serial.print(tmpA);
+      printf("%lf, ", *Px);
+      for (int i = 0; i < 10; i++)
+        tmpA[i] = 0; // 文字列を初期化
+      numa = 0;
+      Px++;
+      while (((tmpchar = fgetc(myFile)) != '\r' && tmpchar != ';') && tmpchar != '/') { // 改行コードかセミコロン，スラッシュが来るまで繰り返し
+        tmpB[numb] = tmpchar;
+        numb++;
+      }
+      if (tmpchar == ';') {
+        file_end = true;
+      } else if (tmpchar == '/') { // コメントアウト対応
+        while ((tmpchar = fgetc(myFile)) != '\n');
+      } else {
+        fgetc(myFile); // "\n"を捨てるため
+      }
+      *Py = str2double(tmpB, numb); //関数でdoubleに変換
+      point_num++;
+
+      printf("%lf\n", *Py);
+      // Serial.println(tmpB);
+      for (int i = 0; i < 10; i++)
+        tmpB[i] = 0;
+      numb = 0;
+      Py++;
+    }
+    // Serial.print("path done! ");
+    file_end = false;
+    //   the file:
+    fclose(myFile);
+  } else {
+    // if the file didn't open, print an error:
+    // Serial.println("error opening test.txt");
+    return -2;
+  }
+  printf("point num: %d\n", point_num);
+
+  // 速度設定用ファイルからデータを読み込む
+  printf("openning... %s\n", velFile);
+  myFile = fopen(velFile, "r");
+  if (myFile) {
+    while (!file_end && !feof(myFile)) {
+      while ((tmpchar = fgetc(myFile)) != ',') {
+        tmpA[numa] = tmpchar;
+        numa++;
+      }
+      *vel = str2double(tmpA, numa); //関数でdoubleに変換
+      // Serial.print(tmpA);
+      printf("%lf, ", *vel);
+      for (int i = 0; i < 10; i++)
+        tmpA[i] = 0;
+      numa = 0;
+      vel++;
+      //////////////////////////////////
+      while ((tmpchar = fgetc(myFile)) != ',') {
+        tmpB[numb] = tmpchar;
+        numb++;
+      }
+      *angle = str2double(tmpB, numb); //関数でdoubleに変換
+      // Serial.print(tmpA);
+      printf("%lf, ", *angle);
+      for (int i = 0; i < 10; i++)
+        tmpB[i] = 0;
+      numb = 0;
+      angle++;
+      //////////////////////////////////
+      while ((tmpchar = fgetc(myFile)) != ',') {
+        tmpC[numc] = tmpchar;
+        numc++;
+      }
+      *mode = str2uint(tmpC, numc); //関数でdoubleに変換
+      // Serial.print(tmpA);
+      printf("%d, ", *mode);
+      for (int i = 0; i < 10; i++)
+        tmpC[i] = 0;
+      numc = 0;
+      mode++;
+      //////////////////////////////////
+      while ((tmpchar = fgetc(myFile)) != ',') {
+        tmpD[numd] = tmpchar;
+        numd++;
+      }
+      *count = str2uint(tmpD, numd); //関数でdoubleに変換
+      // Serial.print(tmpA);
+      printf("%d, ", *count);
+      for (int i = 0; i < 10; i++)
+        tmpD[i] = 0;
+      numd = 0;
+      count++;
+      //////////////////////////////////
+      while (((tmpchar = fgetc(myFile)) != '\r' && tmpchar != ';') &&
+             tmpchar != '/') {
+        tmpE[nume] = tmpchar;
+        nume++;
+      }
+      if (tmpchar == ';') {
+        file_end = true;
+      } else if (tmpchar == '/') { // コメントアウト対応
+        while ((tmpchar = fgetc(myFile)) != '\n');
+      } else {
+        fgetc(myFile); // "\n"を捨てるため
+      }
+      *tbe = str2double(tmpE, nume); //関数でdoubleに変換
+      path_num++;
+
+      // Serial.print(tmpB);
+      printf("%lf\n", *tbe);
+      for (int i = 0; i < 10; i++)
+        tmpE[i] = 0;
+      nume = 0;
+      tbe++;
+    }
+    // Serial.println("vel/angle done!");
+    // close the file:
+    fclose(myFile);
+  } else {
+    // if the file didn't open, print an error:
+    // Serial.println("error opening test.txt");
+    return -3;
+  }
+
+  if ((int)((point_num - 2) / 3) >= (path_num - 1)) {
+    return path_num - 1;
+  }
+  return -4;
+}
+
+double mySDclass::str2double(char *str, int num) {
+  double ret = 0.0;
+  bool minus = false;
+  int m = 0, keta;
+
+  // マイナス符号が付いているかチェック
+  if (str[0] == '-') {
+    minus = true;
+    m++;
+  }
+
+  // 何桁あるかを確認
+  keta = m;
+  while ((str[keta] != '.') && (keta < num)) {
+    keta++;
+  }
+  keta = keta - (m + 1);
+
+  // 整数部を変換
+  for (int i = m; i <= (keta + m); i++) {
+    if (str[i] >= 48 && str[i] <= 57) {
+      ret += (double)(str[i] - 48) * pow(10.0, keta - (i - m));
+    }
+  }
+
+  // 小数部を変換
+  int n = -1;
+  for (int i = keta + m + 2; i < num; i++) {
+    if (str[i] >= 48 && str[i] <= 57) {
+      ret += (double)(str[i] - 48) * pow(10.0, n);
+      n--;
+    }
+  }
+  if (minus)
+    return -1.0 * ret;
+  return ret;
+}
+
+int mySDclass::str2uint(char *str, int num) {
+  num--;
+  int ret = 0;
+  // bool minus = false;
+
+  // 整数部を変換
+  for (int i = 0; i <= num; i++) {
+    if (str[i] >= 48 && str[i] <= 57) {
+      ret += (double)(str[i] - 48) * pow(10.0, num - i);
+    }
+  }
+
+  return ret;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDclass.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,28 @@
+#ifndef SDCLASS_h
+#define SDCLASS_h
+
+#include "mbed.h"
+//#include <SD.h>
+#include "define.h"
+#include "FATFileSystem.h"
+#include "SDBlockDevice.h"
+#include <string>
+
+class mySDclass{
+public:
+    mySDclass();
+    
+    bool SD_enable = false;
+    string logFileName;
+    
+    int init();
+    int make_logfile();
+    int write_logdata(string);
+
+    int path_read(int, double*, double*, double*, double*, int*, int*, double*);
+
+    double str2double(char*, int);
+    int str2uint(char* str, int num);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/phaseCounterPeach.cpp	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,113 @@
+#include "phaseCounterPeach.h"
+
+PhaseCounter::PhaseCounter(int xch)
+{
+    g_ch = xch;
+    ch_available = false;
+    pre_rawcount = ENC_INIT_VAL;
+    encount = 0;
+}
+
+int PhaseCounter::getCount(){
+	unsigned short int rawcount;
+	int diff;
+	
+	if(ch_available){
+	    switch(g_ch){
+	        case 1:
+	        rawcount =  MTU2.TCNT_1;
+	        break;
+	        case 2:
+	        rawcount =  MTU2.TCNT_2;
+	        break;
+			default:
+			rawcount = 0;
+			break;
+	    }
+	    
+	    diff = (int)rawcount - (int)pre_rawcount; // 差分を計算
+	    if(diff > ENC_INIT_VAL){  // マイナス方向にゼロ点回ったとき
+            diff = -(int)pre_rawcount - (0xFFFF - (int)rawcount);
+        }else if(diff < -ENC_INIT_VAL){ // プラス方向にゼロ点回ったとき
+            diff = (int)rawcount + (0xFFFF - (int)pre_rawcount);
+        }
+         // 差分をインクリメントする
+        encount += diff;
+        pre_rawcount = rawcount;
+        return encount;
+	}
+    
+    return 0;
+}
+
+void PhaseCounter::setCount(int ch, int num){
+	
+}
+
+
+void PhaseCounter::init(){
+	CPG.STBCR3 &= ~0x08; //マルチファンクションタイマパルスユニット2へクロックを供給(これをしていないとレジスタを書き換えられない)
+	/***************ピンの設定***************/
+	if(g_ch == 1){
+		/***** ポートの初期化 *****/
+		GPIO.PIBC1 &= ~0x0401; // ポート入力バッファ制御レジスタ 入力バッファ禁止
+		GPIO.PBDC1 &= ~0x0401; // ポート双方向制御レジスタ 双方向モードを禁止
+		GPIO.PM1 |= 0x0401; // ポートモードレジスタ 入力モード
+		GPIO.PMC1 &= ~0x0401; // ポートモード制御レジスタ ポートモード
+		GPIO.PIPC1 &= ~0x0401; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
+		
+		/***** 入力機能のポート設定 *****/
+		GPIO.PBDC1 &= ~0x0401; // ポート双方向制御レジスタ 双方向モードを禁止
+		
+		/***** ポート設定 *****/
+		GPIO.PFC1 |= 0x0400;
+		GPIO.PFCE1 |= 0x0401;
+		//GPIO.PFCAE1 &= !0xC03;
+
+		GPIO.PIPC1 |= 0x0401; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
+		GPIO.PMC1 |= 0x0401; // ポートモード制御レジスタ ポートモード
+	}else if(g_ch == 2){
+		/***** ポートの初期化 *****/
+		GPIO.PIBC1 &= ~0x0802; // ポート入力バッファ制御レジスタ 入力バッファ禁止
+		GPIO.PBDC1 &= ~0x0802; // ポート双方向制御レジスタ 双方向モードを禁止
+		GPIO.PM1 |= 0x0802; // ポートモードレジスタ 入力モード
+		GPIO.PMC1 &= ~0x0802; // ポートモード制御レジスタ ポートモード
+		GPIO.PIPC1 &= ~0x0802; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
+		
+		/***** 入力機能のポート設定 *****/
+		GPIO.PBDC1 &= ~0x0802; // ポート双方向制御レジスタ 双方向モードを禁止
+		
+		/***** ポート設定 *****/
+		GPIO.PFC1 |= 0x0800;
+		GPIO.PFCE1 |= 0x0802;
+		//GPIO.PFCAE1 &= !0xC03;
+
+		GPIO.PIPC1 |= 0x0802; // ポート IP 制御レジスタ　入出力はPMn.PMnmビットによって制御されます
+		GPIO.PMC1 |= 0x0802; // ポートモード制御レジスタ ポートモード
+	}
+
+    /***************MTU1 (MTCLKA, MTCLKB)の設定***************/
+    if(g_ch == 1){
+	    MTU2.TSTR &= ~0x02; //MTU1.TCNTのカウント停止
+	    MTU2.TCR_1 = 0;  //よくわからないけど，ここはゼロにしておけばOK?
+	    MTU2.TMDR_1 |= 0x04; //位相計数モード1 4逓倍のカウント読み取り
+	    MTU2.TCNT_1 = ENC_INIT_VAL; //カウントを初期化
+	    MTU2.TIOR_1 |= 0xAA;  //両エッジでインプットキャプチャ
+	    MTU2.TSTR |= 0x02;  //MTU1.TCNTのカウント開始
+	    
+	    ch_available = true;
+    }
+    
+    /***************MTU2 (MTCLKC, MTCLKD)の設定***************/
+    if(g_ch == 2){
+	    MTU2.TSTR &= ~0x04; //MTU1.TCNTのカウント停止
+	    MTU2.TCR_2 = 0;  //よくわからないけど，ここはゼロにしておけばOK?
+	    MTU2.TMDR_2 |= 0x04; //位相計数モード1 4逓倍のカウント読み取り
+	    MTU2.TCNT_2 = ENC_INIT_VAL; //カウントを初期化	    
+	    MTU2.TIOR_2 |= 0xAA;  //両エッジでインプットキャプチャ
+      	MTU2.TSTR |= 0x04;  //MTU2.TCNTのカウント開始
+	    
+	    ch_available = true;
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/phaseCounterPeach.h	Mon Jan 24 03:12:54 2022 +0000
@@ -0,0 +1,27 @@
+#ifndef PHASECOUNTER_h
+#define PHASECOUNTER_h
+
+//#include"gr_common/rx63n/iodefine_gcc63n.h"
+#include "RZ_A1H.h"
+
+#define ENC_INIT_VAL 0x7FFF
+
+class PhaseCounter
+{
+public:
+    PhaseCounter(int);
+    int getCount();
+    void setCount(int ch, int num);
+    void init();
+
+private:
+    bool ch_available;
+    
+    unsigned short int pre_rawcount;
+    int encount;
+    
+    int g_ch;
+};
+
+#endif
+