test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 5:556d5a5e9d24
- Parent:
- 4:fffdb273836e
- Child:
- 6:43708adf2e5d
--- a/main.cpp Sat Feb 09 14:18:15 2019 +0000 +++ b/main.cpp Sun Feb 10 06:07:02 2019 +0000 @@ -4,126 +4,211 @@ #include "mbed.h" #include "KondoServo.h" -#define Mastar; +#define Mastar ; //#define Slave; -const float Pi = 3.141592; -const float kRadToDegree = 180.0 / Pi; - //定義 namespace Quadruped { - class Leg; - namespace Motion{} - namespace WalkingPattern{} -} +const float Pi = 3.141592; +const float kRadToDegree = 180.0 / Pi; +const float tickerTime = 0.006f; //Ticker間隔 + +const float omega = 3.1415f * 2.5f; //角速度 +const float offsetDegree = 0.0f; +const float offsetY = 0.15f; + +float Stride_time_s = 1.0f;//床についている時間 +float Stride_length_m = 0.1f;//歩幅 +void SetStrideMotion(float stride_time_s, float stride_length_m); +float Rising_time_s = 0.2f;//脚を上げている時間 +float Width_m = 0.03f;//脚を上げる高さ +void SetRisiongMotion(float rising_time_s, float width_m); +//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。 +int WalkMode = 0; +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; +void SetWalkMode(int 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 { - //class Mastar; - //class Slave; +//class Mastar; +//class Slave; } //実装 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_; +//脚の定義 +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); - }; - 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; + 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,float stride_length_m,float &targetX, float &targetY); //引数に歩幅 +void Rising(float time_s,float width_m,float &targetX, float &targetY); //引数に上げる高さ +} // namespace Motion + +namespace WalkingPattern +{ +using namespace Motion; +void Straight(); +void TurnLeft(); +void TurnRight(); +void Climb(); +void Overcoming(); //段差、紐乗り越え動作 +} // namespace WalkingPattern - 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]; - } +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; - namespace Motion - { - void Slide(Leg leg,float stride_m);//引数に歩幅 - void Rising(Leg leg,float width_m);//引数に上げる高さ - } - void Motion::Slide(Leg leg,float stride_m) - { - static float time_s = 0.0f; - - } - void Motion::Rising(Leg leg,float width_m) - { - static float time_s = 0.0f; - - } - - namespace WalkingPattern - { - using namespace Motion; - void Straight(); - void TurnRight(); - void TurnLeft(); - void Climb(); - void Overcoming();//段差、紐乗り越え動作 - } + 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,float stride_length_m,float &targetX, float &targetY) +{ + +} + +void Motion::Rising(float time_s,float width_m,float &targetX, float &targetY) +{ + +} + +void WalkingPattern::Straight() +{ + static float time_s = 0.0f; + float walkPeriod = Stride_time_s+Rising_time_s; + float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f}; + for(int i=0;i<4;++i) + { + float thisLegTime = time_s + offsetTime[i]; + if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, Stride_length_m, targetX[i],targetY[i]); + else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s, Width_m,targetX[i],targetY[i]); + else Motion::Stride(thisLegTime-walkPeriod, Stride_length_m,targetX[i],targetY[i]); + } + time_s += tickerTime*2.0f; + if(time_s >= walkPeriod) time_s == 0.0f; + +} +void WalkingPattern::TurnLeft() +{ + static float time_s = 0.0f; + +} +void WalkingPattern::TurnRight() +{ + static float time_s = 0.0f; + +} +void WalkingPattern::Climb() +{ + static float time_s = 0.0f; + +} +void WalkingPattern::Overcoming() +{ + static float time_s = 0.0f; + +} + +void CalcWalk() +{ + switch(WalkMode) + { + case straight: + WalkingPattern::Straight(); + break; + case turnleft: + WalkingPattern::TurnLeft(); + break; + case turnright: + WalkingPattern::TurnRight(); + break; + case climb: + WalkingPattern::Climb(); + break; + case overcoming: + WalkingPattern::Overcoming(); + break; + } +} +} // namespace Quadruped //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述 using namespace Quadruped; Leg leg1(p9, p10); -Leg leg2(p28,p27); -CAN can1(p30,p29); +Leg leg2(p28, p27); +CAN can1(p30, p29); Ticker fliper; -const float tickerTime = 0.006f; //Ticker間隔 -const float omega = 3.1415f * 2.5f; //角速度 -const float offsetDegree = 0.0f; -const float offsetY = 0.15f; + void F_t(float t, float &tarX, float &tarY) { @@ -149,6 +234,7 @@ leg1.MoveServo(servo_num); leg2.MoveServo(servo_num); ++servo_num; + if (servo_num > 1) //両サーボ出力したら { //目標角の更新 @@ -158,6 +244,7 @@ servo_num = 0; } time_s += tickerTime; + } int main() {