test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 25:5d0977d2d79d
- Parent:
- 20:70cc6083e9c7
--- a/main.cpp Fri Feb 15 06:46:39 2019 +0000 +++ b/main.cpp Wed Feb 20 01:00:23 2019 +0000 @@ -20,13 +20,15 @@ const double kServoValToDegree = 270.0 / (11500 - 3500); //サーボの正負と座標系の正負の補正.足で一セット。 const int kServoSign[2][2] = {{ - 1, - -1, - }, - { - -1, - 1, - }}; + 1, + -1, + + }, + { + -1, + 1, + } +}; //欲しい座標系0度でのサーボのICSマネージャーの値 const double kOriginDegree[2][2] = { { @@ -65,14 +67,17 @@ } int main() { - printf("When you push any key, this robot starts.\r\n"); + printf("------------------------New Session Start------------------------.\r\n"); + printf("Ready to stand up. Press any key.\r\n"); while (pc.readable() == 0) //キーボード押したらスタート ; - printf("stand up for 1 s\r\n"); - //stand時の足の軌道設定. Orbit stand_orbit[4]; - float stand_offset_y_m = 0.2; + + //機体高さ + float stand_offset_y_m = 0.18; + + stand_orbit[0].SetStandParam(stand_offset_y_m); for (int i = 0; i < 4; i++) //全部足を同じにしてる stand_orbit[i] = stand_orbit[0]; @@ -89,38 +94,49 @@ Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s); float dist_m = 10; //収束判定用。現在は回すループの数 //動作開始 + Move(stand, leg, dist_m); + wait(1); + printf("Ready to walk. Press [a] to start.\r\n"); + while(1) { + char c = pc.getc(); + if(c == 'a') {printf("Waliking Session Start.\r\n");break;} +} + + - DEBUG("move start\r\n"); - //取り敢えず歩行したパラメータ - //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, offset_x_m = 0, offset_y_m = 0.16f; - //曲げてみる - float stridetime_s = 2, risetime_s = 0.5, - stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03, - offset_x_m = -0.05, offset_y_m = stand_offset_y_m; - //軌道の作成 - //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる - Orbit orbit[4]; - orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m); - orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m); - orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m); - orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m); - //4足の軌道と位相ずれOffsetTime_sをまとめる - float offset_time_s[4] = { - 0, - orbit[0].GetOneWalkTime() * 0.5, - orbit[0].GetOneWalkTime() * 0.5, - orbit[0].GetOneWalkTime() * 0, - }; - //4つの足のorbit, 位相を代入して歩行パターンを作成している - //このインスタンスはlegそれぞれの計算を行う役割を担う - //orbitはここでしか使わないあくまで鍵のような扱い - Walk walk(orbit, offset_time_s, kCycleTime_s); - dist_m = 500000; //収束判定用。取り敢えず今はループ数。 - //動く - Move(walk, leg, dist_m); - DEBUG("program end\r\n"); +DEBUG("move start\r\n"); +//取り敢えず歩行したパラメータ +//stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, offset_x_m = 0, offset_y_m = 0.16f; +//曲げてみる +float stridetime_s = 0.3, risetime_s = 0.15, + stride_r_m = 0.1, stride_l_m = 0.1, height_m = 0.03, + offset_x_m = 0 + , offset_y_m = stand_offset_y_m; +//軌道の作成 +//代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる +Orbit orbit[4]; +orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m); +orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m); +orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m); +orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m); +//4足の軌道と位相ずれOffsetTime_sをまとめる +float offset_time_s[4] = { + 0, + orbit[0].GetOneWalkTime() * 0.5, + orbit[0].GetOneWalkTime() * 0.5, + orbit[0].GetOneWalkTime() * 0, +}; +//4つの足のorbit, 位相を代入して歩行パターンを作成している +//このインスタンスはlegそれぞれの計算を行う役割を担う +//orbitはここでしか使わないあくまで鍵のような扱い +Walk walk(orbit, offset_time_s, kCycleTime_s); +dist_m = 500000; //収束判定用。取り敢えず今はループ数。 +//動く + +Move(walk, leg, dist_m); +DEBUG("program end\r\n"); } //到達判定が来ない間同じ歩行方法でループ @@ -130,8 +146,7 @@ timer.start(); int is_arrived = 0; int count = 0; - while (is_arrived == 0) - { + while (is_arrived == 0) { float time_s = timer.read(); //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断 is_arrived = IsArrived(count, (int)dist_m); @@ -153,8 +168,8 @@ float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s); if (rest_time_s > 0) wait(rest_time_s); - else - { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。 + else { + //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。 DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s); led[0] = 1; }