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 12:56:00 2019 +0000
Parent:
10:7a340c52e270
Child:
12:2ac37fe6c3bb
Commit message:
ver2

Changed in this revision

MoveServoOfLeg/MoveServoOfLeg.cpp Show diff for this revision Revisions of this file
MoveServoOfLeg/MoveServoOfLeg.h Show diff for this revision Revisions of this file
OneLeg/OneLeg.cpp Show annotated file Show diff for this revision Revisions of this file
OneLeg/OneLeg.h Show annotated file Show diff for this revision Revisions of this file
ToePosi/ToePosi.cpp Show diff for this revision Revisions of this file
ToePosi/ToePosi.h Show diff for this revision Revisions of this file
Walk/Walk.cpp Show annotated file Show diff for this revision Revisions of this file
Walk/Walk.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
--- a/MoveServoOfLeg/MoveServoOfLeg.cpp	Mon Feb 11 04:07:51 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-#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];
-}
-
--- a/MoveServoOfLeg/MoveServoOfLeg.h	Mon Feb 11 04:07:51 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-#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/OneLeg/OneLeg.cpp	Mon Feb 11 12:56:00 2019 +0000
@@ -0,0 +1,58 @@
+#include "OneLeg.h"
+#include "mbed.h"
+const float M_PI = 3.141592;
+//注:未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない
+float OneLeg::GetX_m()
+{
+    return x_m_;
+}
+float OneLeg::GetY_m()
+{
+    return y_m_;
+}
+void OneLeg::SetXY_m(float x_m, float y_m)
+{
+    CalServoRad(x_m, y_m);
+    if (isnan(GetRad(0)) || isnan(GetRad(1)))
+        printf("error:(x,y) = (%f,%f) is out of range ", x_m, y_m);
+    else
+    {
+        x_m_ = x_m;
+        y_m_ = y_m;
+    }
+}
+
+OneLeg::OneLeg(float between_servo_half_m,
+               float leglength1, float leglength2)
+{
+    rad_[0] = 0;
+    rad_[1] = M_PI;
+    between_servo_half_m_ = between_servo_half_m;
+    leglength1_ = leglength1;
+    leglength2_ = leglength2;
+}
+void OneLeg::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 OneLeg::SetRad(float rad, int servo_num)
+{
+    rad_[servo_num] = rad;
+}
+float OneLeg::GetRad(int servo_num)
+{
+    return rad_[servo_num];
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OneLeg/OneLeg.h	Mon Feb 11 12:56:00 2019 +0000
@@ -0,0 +1,28 @@
+#ifndef INCLUDED_ONELEG_H
+#define INCLUDED_ONELEG_H
+
+
+///足先の座標を保存するクラス。x,yやサーボの角度の保存を行う。他の足を考慮した処理は別のクラスに任せる。
+class OneLeg
+{
+    float rad_[2]; //足についてるサーボの目標角度。
+    float x_m_;
+    float y_m_;
+    float between_servo_half_m_;
+    float leglength1_;
+    float leglength2_;
+    void CalServoRad(float x_m, float y_m);
+
+  public:
+    OneLeg(float between_servo_half_m,
+           float leglength1, float leglength2);
+    void MoveServo(int servo_num);
+    void SetRad(float rad, int servo_num);
+    float GetRad(int servo_num);
+    float GetX_m();
+    float GetY_m();
+    void SetXY_m(float x_m, float y_m);
+};
+
+
+#endif
\ No newline at end of file
--- a/ToePosi/ToePosi.cpp	Mon Feb 11 04:07:51 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,39 +0,0 @@
-#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 = offsety_m_;
-    if (phase < M_PI) //空中にいるとき(最初の半周期)のみy座標は変化
-        y_m += sin(phase) * height_m_;
-    return y_m;
-}
-
-ToePosi::ToePosi(float stride_m, float height_m, float offsety_m)
-{
-    phase_ = 0;
-    stride_m_ = stride_m;
-    height_m_ = height_m;
-    offsety_m_ = offsety_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;
-}
--- a/ToePosi/ToePosi.h	Mon Feb 11 04:07:51 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-#ifndef INCLUDED_TOEPOSI_H
-#define INCLUDED_TOEPOSI_H
-//足先の位置に関するもの全般
-class ToePosi
-{
-    float phase_; //足先の位相
-    float stride_m_;
-    float height_m_;
-    float offsety_m_;
-  public:
-    ToePosi(float stride_m, float height_m, float offsety_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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk/Walk.cpp	Mon Feb 11 12:56:00 2019 +0000
@@ -0,0 +1,66 @@
+#include "Walk.h"
+#include "math.h"
+const float M_PI = 3.141592;
+Orbit::Orbit(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m)
+{
+    stridetime_s_ = stridetime_s;
+    risetime_s_ = risetime_s;
+    stride_m_ = stride_m;
+    height_m_ = height_m;
+    ground_m_ = ground_m;
+}
+//着地中の動き.直線軌道.等速
+OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
+{
+    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5;
+    float y_m = ground_m_;
+    leg.SetXY_m(x_m, y_m);
+    return leg;
+}
+//空中の動き.楕円軌道
+OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
+{
+    float rad = M_PI * phasetime_s / risetime_s_ + M_PI;
+    float x_m = stride_m_ * 0.5 * cos(rad);
+    float y_m = height_m_ * sin(rad) + ground_m_;
+    leg.SetXY_m(x_m, y_m);
+    return leg;
+}
+//楕円軌道
+OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
+{
+    if (phasetime_s < stridetime_s_)
+        return StrideLine_(leg, phasetime_s);
+    else
+        return RiseEllipse_(leg, phasetime_s);
+}
+//将来的には引数などで軌道の種類を変えて出力できるようにしたい。
+OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s)
+{
+    return OrbitEllipse_(leg, phasetime_s);
+}
+float Orbit::GetOneWalkTime()
+{
+    return stridetime_s_ + risetime_s_;
+}
+
+Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        orbit_[i] = orbit[i];
+        offsettime_s_[i] = offsettime_s[i];
+    }
+    cycletime_s_ = cycletime_s;
+}
+void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
+{
+    phasetime_s_ += cycletime_s_;
+    for (int i = 0; i < 4; i++)
+    {
+        float phasetime_s = phasetime_s_ + offsettime_s_[i];
+        while (phasetime_s > orbit_[i].GetOneWalkTime())
+            phasetime_s -= orbit_[i].GetOneWalkTime();
+        leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s);
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk/Walk.h	Mon Feb 11 12:56:00 2019 +0000
@@ -0,0 +1,36 @@
+#ifndef INCLUDED_WALK_H
+#define INCLUDED_WALK_H
+#include "OneLeg.h"
+
+//足の軌道を設定するクラス。
+class Orbit
+{
+    float stridetime_s_;
+    float risetime_s_;
+    float stride_m_;
+    float height_m_; //足の上げ幅
+    float ground_m_; //着地するときの高さ
+    OneLeg StrideLine_(OneLeg leg, float phasetime_s);
+    OneLeg RiseEllipse_(OneLeg leg, float phasetime_s);
+    OneLeg OrbitEllipse_(OneLeg leg, float phasetime_s);
+
+  public:
+    Orbit(){};
+    Orbit(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m);
+    OneLeg GetOrbit(OneLeg leg, float phasetime_s);
+    float GetOneWalkTime(); //足一周の時間
+};
+
+//4足の足の動かし方を保存するクラス
+class Walk
+{
+    float phasetime_s_;
+    float offsettime_s_[4];
+
+  public:
+    Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s);
+    float cycletime_s_;
+    Orbit orbit_[4];
+    void Cal4LegsPosi(OneLeg (&leg)[4]);
+};
+#endif
\ No newline at end of file
--- a/main.cpp	Mon Feb 11 04:07:51 2019 +0000
+++ b/main.cpp	Mon Feb 11 12:56:00 2019 +0000
@@ -1,77 +1,65 @@
-//NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。
+//NHK2019MR2 馬型機構プログラム.
 #include "mbed.h"
 #include "pinnames.h"
-#include "ToePosi.h"        //個々の足先の位置を決めるプログラム
-#include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
-#define DEBUG
+#include "KondoServo.h"
+#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
+#include "Walk.h"   //歩き方に関するファイル
 
 ////////////調整すべきパラメータ.全てここに集めた。
-const float kCycleTime_s = 0.02f;              //計算周期
-const float kStride_m = 0.05f;                 //歩幅
-const float kHeight_m = 0.03f;                 //足の上げ幅
-const float kOffsetY_m = 0.02f;                //足のデフォルト高さ
+const float kCycleTime_s = 0.01f;              //計算周期
 const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
 const float kLegLength1 = 0.1f;
 const float kLegLength2 = 0.2f;
-const float kServoSpan_ms = 0;     //サーボの送信間隔
-const float kVelocity_m_s = 0.01f; //進む速度(適当)
-const float kDist_m = 10;          //歩行中に進む距離(適当)
+const float kServoSpan_ms = 5; //サーボの送信間隔
 ///////////////
 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),
+KondoServo servo[2] = {
+    KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
+    KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
+};
+OneLeg leg[4] = {
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
 };
 DigitalOut led(LED1);
 Serial pc(USBTX, USBRX);
-
 const float M_PI = 3.141592;
-//歩き方のパターン
-enum WalkingPattern
-{
-    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);
-
+const float kRadToDegree = 180.0 / M_PI;
+void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
+void MoveServo(OneLeg leg, int legnum, int servo_id);
 int main()
 {
-    printf("program start\r\n");
-    //現在のそれぞれの足の位置を保存するもの
-    ToePosi now[4] = {
-        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
-        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
-        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
-        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
-    };
-    //直進
-    printf("please put some key\r\n");
-    while (pc.readable() == 0)
+    printf("When you push any key, this robot starts.\r\n");
+    while (pc.readable() == 0) //キーボード押したらスタート
         ;
     printf("move start\r\n");
-    Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
+    //各足の軌道の設定.今回は全部同じにすることで直線を描く。
+    float stridetime_s = 1, risetime_s = 0.3, stride_m = 0.2f, height_m = 0.03f, ground_m = 0.14f;
+    Orbit orbit[4] = {
+        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
+        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
+        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
+        Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m),
+    };
+    //4足の軌道と位相ずれOffsetTime_sをまとめる
+    float offset_time_s[4] = {
+        0,
+        orbit[0].GetOneWalkTime() * 0.5,
+        0,
+        orbit[0].GetOneWalkTime() * 0.5,
+    };
+    Walk straight(orbit, offset_time_s, kCycleTime_s);
+    //実際にWalkの指示通りに動かす
+    float dist_m = 0.5f;
+    Move(straight, leg, dist_m);
+
     printf("program end\r\n");
 }
 
-void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
+void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
 {
-    //patternを見て、使う関数を決定.関数ポインタを利用
-    void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float);
-    switch (pattern)
-    {
-    case STRAIGHT_TROT:
-        Cal4LegsPhase = CalPhaseStraight;
-        //収束判定関数もここでpatternによって変える必要あるかも。
-        break;
-    default:
-        printf("error:there is no func of the pattern");
-    }
-
-    ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。
     timer.reset();
     timer.start();
     int is_arrived = 0;
@@ -81,24 +69,19 @@
         //注:未実装。到着したかの判定.LRFからのデータが必要?
         // is_arrived = IsArrived();
 
-        //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
-        Cal4LegsPhase(now[0], target, maxvelocity_m_s);
+        //4本の足それぞれの足先サーボ角度更新
+        WalkWay.Cal4LegsPosi(leg);
         //注:未実装。slave_mbed分の足の目標位置を送信
 
-        //自身が動かす足のサーボ角度更新
-        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);
+        MoveServo(leg[0], 0, 0);
+        MoveServo(leg[1], 1, 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);
+        MoveServo(leg[0], 0, 1);
+        MoveServo(leg[1], 1, 1);
+        printf("%f,%f\r\n", leg[0].GetRad(0), leg[1].GetRad(0));
+        //計算周期がWalkWay.cycletime_s_になるようwait
+        float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
         if (rest_time_s > 0)
             wait(rest_time_s);
         else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。
@@ -106,15 +89,13 @@
     }
 }
 
-void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s)
+void MoveServo(OneLeg leg, int legnum, int servo_id)
 {
-    //指定速度で進むための周期 = 平均速度*時間/歩幅 * 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);
-}
+    float degree = leg.GetRad(servo_id) * kRadToDegree;
+    //servo1は反転させる
+    if (servo_id == 0)
+        degree += 90;
+    else
+        degree = 270 - degree;
+    servo[legnum].set_degree(servo_id, degree);
+}
\ No newline at end of file