test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
- Committer:
- shimizuta
- Date:
- 2019-02-08
- Revision:
- 2:a92568bdeb5c
- Parent:
- 1:acaa3d7ede4c
- Child:
- 3:bcae0bb64b81
File content as of revision 2:a92568bdeb5c:
#include "mbed.h" #include "KondoServo.h" ////////////////Legクラス。下にmain文がある。 const float Pi = 3.141592; const float kRadToDegree = 180.0 / Pi; //const float kDegreeToRad = Pi / 180.0; const float LegLength1 = 0.1f; const float LegLength2 = 0.2f; class Leg { float rad_[2]; int id_[2]; //パラメータ。実際の機体に合わせていじる static const float dist_between_servo_half_m_ = 0.06f * 0.5; 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; 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 leg(p9, p10); 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) { float theta = -omega * t + offsetDegree; if (sin(theta) <= 0.0f) { tarX = 0.5f * LegLength2 * cos(theta); //0.5 tarY = 0.15f * LegLength2 * sin(theta); //0.15 } else { tarX = 0.5f * LegLength2 * cos(theta); tarY = 0.0f * LegLength2 * sin(theta); } } void cb_timer() { static int servo_num = 0; static float time_s = 0.0f; float target_x_m = 0; float target_y_m = 0; //出力。片サーボずつ leg.MoveServo(servo_num); ++servo_num; if (servo_num > 1) //両サーボ出力したら { //目標角の更新 F_t(time_s, target_x_m, target_y_m); leg.CalServoRad(target_x_m, target_y_m + offsetY); servo_num = 0; } time_s += tickerTime; } int main() { float target_x_m = 0; float target_y_m = 0; F_t(0, target_x_m, target_y_m); leg.CalServoRad(target_x_m, target_y_m + offsetY); fliper.attach(&cb_timer, tickerTime); }