serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp

Committer:
yuto17320508
Date:
3 months ago
Revision:
8:21b932c4e6c5
Parent:
7:72c80a7c20d6
Child:
9:905f93247688

File content as of revision 8:21b932c4e6c5:

//mbed間の同期を、master-slaveで行いたい。
//masetrで4つの脚の動きをすべて決定,
//その動きをslaveに送る。

#include "mbed.h"
#include "KondoServo.h"
#define Mastar;
//#define Slave;
#define DebugMode;

//各値
namespace Parameters
{
    //パラメータ
const float Pi = 3.14;
const float kRadToDegree = 180.0 / Pi;
const float tickerTime = 0.007f;//Ticker間隔
float OffsetY = 0.15f;
float Stride_time_s = 0.8f;//床についている時間 1.0f
float Stride_length_m = 0.05f;//歩幅
float Rising_time_s = 0.2f;//脚を上げている時間 0.2f
float Width_m = 0.03f;//脚を上げる高さ
static const int straight = 0;
static const int turnleft = 1;
static const int turnright = 2;
static const int climb = 3;
static const int overcoming = 4;
}
//定義 
namespace Quadruped//4脚ロボット
{
using namespace Parameters;
//USERが直接制御に使う関数
void SetOffsetY(float offsetY){OffsetY=offsetY;}
void SetStrideMotion(float stride_time_s, float stride_length_m){Stride_time_s=stride_time_s;Stride_length_m=stride_length_m;}///////////////////////////////////設定
void SetRisiongMotion(float rising_time_s, float width_m){Rising_time_s=rising_time_s;Width_m =width_m;}////////////////////////////////////////設定
//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。
int WalkMode = 0;
void SetWalkMode(int mode){WalkMode = mode;}
//4つの脚のtargetを保存する Legクラスで後で使えるようにしておく
float TargetX[4];
float TargetY[4];
//脚の定義 位置を入力するとその方向にサーボが動く
class Leg;
//脚ごとのモーション これを組み合わせる
namespace Motion{}
//モーションを組み合わせて歩行パターンを形成する
//前進、旋回など
namespace WalkingPattern{}
//パターン関数を呼び出す 一つの関数でいいようにswitchする
void CalcWalk(int legNum, Leg leg);
} // namespace Quadruped

namespace CanConnector
{
//Maserとslaveの定義 同じ関数名で動くようにしたい
class MastarConnector;//cansendするマイコンの場合
class SlaveConnector;//canreadするマイコンの場合
}

//実装
namespace Quadruped
{
//脚の定義
class Leg
{
    float rad_[2];
    int id_[2];
    //パラメータ。実際の機体に合わせていじる
    static const float dist_between_servo_half_m_ = 0.06f * 0.5;
    static const float LegLength1 = 0.1f;
    static const float LegLength2 = 0.2f;
    KondoServo servo_;

  public:
    Leg(PinName pin_serial_tx, PinName pin_serial_rx);
    void MoveServo(int servo_num);
    void CalServoRad(float x_m, float y_m);
    void SetRad(float rad, int servo_num);
    float GetRad(int servo_num);
};

namespace Motion
{
void Stride(float time_s,int legnum); 
void Rising(float time_s,int legnum); 
void Stop(int legnum);
} // namespace Motion

namespace WalkingPattern
{
using namespace Motion;
void Straight(int servo_num);
void TurnLeft(int servo_num);
void TurnRight(int servo_num);
void Climb(int servo_num);
void Overcoming(int servo_num); //段差、紐乗り越え動作
} // namespace WalkingPattern

Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
{
    rad_[0] = 0;
    rad_[1] = Pi;
};
void Leg::MoveServo(int servo_num)
{
    float degree = GetRad(servo_num) * kRadToDegree;
    //servo1は反転させる
    if (servo_num == 0)
        degree += 90;
    else
        degree = 270 - degree;
    servo_.set_degree(servo_num, degree);
}
void Leg::CalServoRad(float x_m, float y_m)
{
    //処理を軽くするために共通部分は先に計算
    float temp_x[] = {x_m + dist_between_servo_half_m_,
                      x_m - dist_between_servo_half_m_};
    float temp_y2 = y_m * y_m;
    float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2;

    float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
    float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
    float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)),
                           atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))};
    for (size_t i = 0; i < 2; i++)
        SetRad(targetTheta[i], i);
}
void Leg::SetRad(float rad, int servo_num)
{
    rad_[servo_num] = rad;
}
float Leg::GetRad(int servo_num)
{
    return rad_[servo_num];
}


void Motion::Stride(float time_s,int legnum)
{
    TargetX[legnum] = (Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f);
    TargetY[legnum] = 0.0f;
    
}

void Motion::Rising(float time_s,int legnum)
{
    TargetX[legnum] = (Stride_length_m/2.0f)*cos(-Pi*time_s/Rising_time_s);
    TargetY[legnum] = Width_m*sin(-Pi*time_s/Rising_time_s);
    
}

float walkPeriod = Stride_time_s+Rising_time_s;//周期
float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差


void WalkingPattern::Straight(int servo_num)
{
    int servo_state = 2*servo_num;
    static float time_s = 0.0f;//共通時間
    for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する
    {
        float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間
        if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i);
        else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,i);
        else Motion::Stride(thisLegTime-walkPeriod,i);
    }
    #ifdef DebugMode
    if(servo_num == 0)
    printf("time_s: %.3f  targetX0: %.3f  targetY0: %.3f\r\n", time_s, TargetX[0], TargetY[0]);
    #endif
    time_s += tickerTime;
    if(time_s >= walkPeriod) 
    {
        time_s = 0.0f;
    }
}
void WalkingPattern::TurnLeft(int servo_num)
{
    //static float time_s = 0.0f;

}
void WalkingPattern::TurnRight(int servo_num)
{
    //static float time_s = 0.0f;

}
void WalkingPattern::Climb(int servo_num)
{
    //static float time_s = 0.0f;
    
}
void WalkingPattern::Overcoming(int servo_num)
{
    //static float time_s = 0.0f;
    
}

void CalcWalk(int servo_num)
{
    switch(WalkMode)
    {
        case straight:
            WalkingPattern::Straight(servo_num);
        break;
        case turnleft:
            WalkingPattern::TurnLeft(servo_num);
        break;
        case turnright:
            WalkingPattern::TurnRight(servo_num); 
        break;
        case climb:
            WalkingPattern::Climb(servo_num);
        break;
        case overcoming:
            WalkingPattern::Overcoming(servo_num);
        break;
    }
}
} // namespace Quadruped

namespace CanConnector
{
    class MastarConnector
    {
        public:
            MastarConnector(PinName rd, PinName td);
            CAN can_;
            void Connect();

    };
    MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){}


    class SlaveConnector
    {
        public:
            SlaveConnector(PinName rd, PinName td);
            CAN can_;
            void Connect();

    };
    SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){}

}

//////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
using namespace Quadruped;
Leg leg1(p9, p10);
Leg leg2(p28, p27);
//CAN can1(p30, p29);
Ticker fliper;
#ifdef Mastar
CanConnector::MastarConnector connector(p30,p29);
#elif Slave
CanConnector::SlaveConnector connector(p30,p29);
#endif

void cb_timer()
{
    static int servo_num = 0;
    //出力。片サーボずつ
    leg1.MoveServo(servo_num);
    leg2.MoveServo(servo_num);
    Quadruped::CalcWalk(servo_num);
    ++servo_num;
    if(servo_num > 1)servo_num = 0;
    
    /*if (servo_num > 1) //両サーボ出力したら
    {
        //目標角の更新
        Quadruped::CalcWalk(servo_num);
        //このマイコンで使う脚だけ計算
        leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
        leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
        
        servo_num = 0;
    }*/
    
}
int main()
{
    Quadruped::CalcWalk(0);
    Quadruped::CalcWalk(1);
    leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
    leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
    fliper.attach(&cb_timer, tickerTime);
}