test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 11:e81425872740
- Parent:
- 10:7a340c52e270
- Child:
- 12:2ac37fe6c3bb
--- 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