serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Files at this revision

API Documentation at this revision

Comitter:
shimizuta
Date:
Mon Feb 11 01:56:49 2019 +0000
Parent:
8:21b932c4e6c5
Child:
10:7a340c52e270
Commit message:
made program readable

Changed in this revision

MoveServoOfLeg/MoveServoOfLeg.cpp Show annotated file Show diff for this revision Revisions of this file
MoveServoOfLeg/MoveServoOfLeg.h Show annotated file Show diff for this revision Revisions of this file
ToePosi/ToePosi.cpp Show annotated file Show diff for this revision Revisions of this file
ToePosi/ToePosi.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
pinnames/pinnames.cpp Show annotated file Show diff for this revision Revisions of this file
pinnames/pinnames.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MoveServoOfLeg/MoveServoOfLeg.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,54 @@
+#include "MoveServoOfLeg.h"
+#include "mbed.h"
+const float M_PI = 3.141592;
+const float kRadToDegree = 180.0 / M_PI;
+MoveServoOfLeg::MoveServoOfLeg(PinName pin_serial_tx, PinName pin_serial_rx,
+                               float between_servo_half_m,
+                               float leglength1, float leglength2)
+    : servo_(pin_serial_tx, pin_serial_rx)
+{
+    rad_[0] = 0;
+    rad_[1] = M_PI;
+    between_servo_half_m_ = between_servo_half_m;
+    leglength1_ = leglength1;
+    leglength2_ = leglength2;
+};
+void MoveServoOfLeg::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 MoveServoOfLeg::CalServoRad(float x_m, float y_m)
+{
+    //処理を軽くするために共通部分は先に計算
+    float temp_x[] = {
+        x_m + between_servo_half_m_,
+        x_m - 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 MoveServoOfLeg::SetRad(float rad, int servo_num)
+{
+    rad_[servo_num] = rad;
+}
+float MoveServoOfLeg::GetRad(int servo_num)
+{
+    return rad_[servo_num];
+}
+
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MoveServoOfLeg/MoveServoOfLeg.h	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,24 @@
+#ifndef INCLUDED_MOVESERVOOFLEG_H
+#define INCLUDED_MOVESERVOOFLEG_H
+#include "KondoServo.h"
+
+///歩く方向を指示する用
+class MoveServoOfLeg
+{
+    float rad_[2]; //足についてるサーボの目標角度。
+    float between_servo_half_m_;
+    float leglength1_;
+    float leglength2_;
+    KondoServo servo_;
+
+  public:
+MoveServoOfLeg(PinName pin_serial_tx, PinName pin_serial_rx,
+                               float between_servo_half_m,
+                               float leglength1, float leglength2);
+    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);
+};
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ToePosi/ToePosi.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,43 @@
+#include "ToePosi.h"
+#include "math.h"
+const float M_PI = 3.141592;
+//足先の軌道を決める関数。要調整。取り敢えず楕円軌道で実装
+float ToePosi::GetX_m()
+{
+    float phase = GetPhase();
+    float x_m = cos(phase) * stride_m_;
+    return x_m;
+}
+float ToePosi::GetY_m()
+{
+    float phase = GetPhase();
+    float y_m;
+    if (phase < M_PI) //最初の半周期は空中
+        y_m = sin(phase) * height_m_;
+    else //後の半周期は着地。直線移動
+        y_m = 0;
+    return y_m;
+}
+
+ToePosi::ToePosi(float stride_m, float height_m)
+{
+    phase_ = 0;
+    stride_m_ = stride_m;
+    height_m_ = height_m;
+}
+float ToePosi::GetPhase()
+{
+    return phase_;
+}
+void ToePosi::SetPhase(float phase)
+{
+    //値域を[0,2pi]にする
+    while (phase > 2 * M_PI)
+        phase -= 2 * M_PI;
+    while (phase < 0)
+        phase += 2 * M_PI;
+
+    phase_ = phase;
+}
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ToePosi/ToePosi.h	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,22 @@
+#ifndef INCLUDED_TOEPOSI_H
+#define INCLUDED_TOEPOSI_H
+//足先の位置に関するもの全般
+class ToePosi
+{
+    float phase_;//足先の位相
+    float stride_m_;
+    float height_m_;
+  public:
+    ToePosi(float stride_m, float height_m);
+    //現在phaseでの足先の位置を計算して返す.
+    //足の軌道の式の実装部分でもある。要調整
+    float GetX_m();
+    float GetY_m();
+
+    //phase:足先の位相。値域は[0,2CalPhase].
+    //0:一番右下で着地している状態とする。0~Piなら空中、Pi~2Piなら着地中で,右下に戻ってくる
+    //GetPhase(),SetPhase()で呼び出しや代入を行う。値域を確実に0~2Piにするため。
+    float GetPhase();
+    void SetPhase(float phase);
+};
+#endif
\ No newline at end of file
--- a/main.cpp	Sun Feb 10 12:38:16 2019 +0000
+++ b/main.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -1,289 +1,115 @@
-//mbed間の同期を、master-slaveで行いたい。
-//masetrで4つの脚の動きをすべて決定,
-//その動きをslaveに送る。
-
+//NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。
 #include "mbed.h"
-#include "KondoServo.h"
-#define Mastar;
-//#define Slave;
-#define DebugMode;
+#include "pinnames.h"
+#include "ToePosi.h"        //個々の足先の位置を決めるプログラム
+#include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
 
-//各値
-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
+////////////調整すべきパラメータ.全てここに集めた。
+const float kCycleTime_s = 0.02f;              //計算周期
+const float kStride_m = 0.05f;                 //歩幅
+const float kHeight_m = 0.03f;                 //足を上げる高さ
+const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
+const float kLegLength1 = 0.1f;
+const float kLegLength2 = 0.2f;
+const float kServoSpan_ms = 50;    //サーボの送信感覚
+const float kVelocity_m_s = 0.01f; //進む速度(適当)
+const float kDist_m = 10;          //歩行中に進む距離(適当)
+///////////////
+Timer timer;
+MoveServoOfLeg moveleg[2] = {
+    MoveServoOfLeg(pin_serial_servo_tx[0], pin_serial_servo_rx[0],
+                   kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    MoveServoOfLeg(pin_serial_servo_tx[1], pin_serial_servo_rx[1],
+                   kBetweenServoHalf_m, kLegLength1, kLegLength2),
+};
+DigitalOut led(LED1);
 
-namespace CanConnector
-{
-//Maserとslaveの定義 同じ関数名で動くようにしたい
-class MastarConnector;//cansendするマイコンの場合
-class SlaveConnector;//canreadするマイコンの場合
-}
-
-//実装
-namespace Quadruped
-{
-//脚の定義
-class Leg
+const float M_PI = 3.141592;
+//歩き方のパターン
+enum WalkingPattern
 {
-    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);
+    STRAIGHT_TROT,
 };
+//歩き方:patternでdist_mだけ進む。
+void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m);
+void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s);
 
-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)
+int main()
 {
-    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];
+    //現在のそれぞれの足の位置を保存するもの
+    ToePosi now[4] = {
+        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m),
+    };
+    //直進
+    Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
 }
 
-
-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)
+void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
 {
-    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を設定する
+    //patternを見て、使う関数を決定.関数ポインタを利用
+    void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float);
+    switch (pattern)
     {
-        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);
+    case STRAIGHT_TROT:
+        Cal4LegsPhase = CalPhaseStraight;
+        //収束判定関数もここでpatternによって変える必要あるかも。
+        break;
+    default:
+        printf("error:there is no func of the pattern");
     }
-    #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) 
+
+    ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。
+    timer.reset();
+    timer.start();
+    int is_arrived = 0;
+    while (is_arrived == 0)
     {
-        time_s = 0.0f;
-    }
-}
-void WalkingPattern::TurnLeft(int servo_num)
-{
-    //static float time_s = 0.0f;
+        float time_s = timer.read();
+        //注:未実装。到着したかの判定.LRFからのデータが必要?
+        //is_arrived = IsArrived();
+
+        //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
+        Cal4LegsPhase(now[0], target, maxvelocity_m_s);
+        //注:未実装。slave_mbed分の足の目標位置を送信
 
-}
-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;
-    
+        //自身が動かす足のサーボ角度更新
+        moveleg[0].CalServoRad(target[0].GetX_m(), target[0].GetY_m());
+        moveleg[1].CalServoRad(target[1].GetX_m(), target[1].GetY_m());
+        //自身が動かす足のサーボを動かす
+        moveleg[0].MoveServo(0);
+        moveleg[1].MoveServo(0);
+        wait_ms(kServoSpan_ms);
+        moveleg[0].MoveServo(1);
+        moveleg[1].MoveServo(1);
+        
+        //現在位置更新
+        for(int i = 0; i < 4;i++)
+            now[i] = target[i];
+        
+        //計算周期をkCycleTime_sにする
+        float rest_time_s = kCycleTime_s - (timer.read() - time_s);
+        if (rest_time_s > 0)
+            wait(rest_time_s);
+        else //計算周期が達成できないときは
+        {
+            led = 1; //LED1を光らせて知らせる。
+            printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
+        }
+    }
 }
 
-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
+void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s)
 {
-    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){}
-
+    //指定速度で進むための周期 = 平均速度*時間/歩幅 * pi(半周期で歩幅分進む)
+    float phase_step = ave_v_m_s * kCycleTime_s / kStride_m * M_PI;
+    //対角線上の足の位相は同じで、反対側は半周期先。
+    float target_phase0 = now_base.GetPhase() + phase_step;
+    float target_phase1 = target_phase0 + M_PI;
+    target[0].SetPhase(target_phase0);
+    target[1].SetPhase(target_phase1);
+    target[2].SetPhase(target_phase0);
+    target[3].SetPhase(target_phase1);
 }
-
-//////////////////////////////////////////////////////////////以上が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);
-}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pinnames/pinnames.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,15 @@
+#include "pinnames.h"
+#include "mbed.h"
+PinName pin_encoder[][2] = {{p12,p11},};
+PinName pin_motor[][2] = {
+    {p21,p22},
+};
+PinName pin_can_rd = p30,
+        pin_can_td = p29;
+PinName pin_serial_servo_tx[] = {p9, p13, p28};
+PinName pin_serial_servo_rx[] = {p10, p14, p27};
+PinName pin_touch_sensor[] = {p8};
+PinName pin_button[] = {p18, p17, p16, p15,p19};
+PinName pin_spi_mosi = p5,
+        pin_spi_miso = p6,
+        pin_spi_sck = p7;
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pinnames/pinnames.h	Mon Feb 11 01:56:49 2019 +0000
@@ -0,0 +1,19 @@
+#ifndef INCLUDED_PINNAMES_H
+#define INCLUDED_PINNAMES_H
+#include "mbed.h"
+/** @file
+@brief pin宣言
+*/
+///[][0]:A相, [][1]:B相のピン
+extern PinName pin_encoder[][2];
+///[][0]:正転ピン, [][1]:逆転ピン
+extern PinName pin_motor[][2];
+extern PinName pin_can_rd, pin_can_td;
+extern PinName pin_serial_servo_tx[], pin_serial_servo_rx[];
+extern PinName pin_button[];
+extern PinName pin_touch_sensor[];
+extern PinName pin_controller[];
+extern PinName pin_spi_mosi,
+       pin_spi_miso,
+       pin_spi_sck;
+#endif
\ No newline at end of file