test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
OneLeg/OneLeg.cpp@50:36741e8ab197, 2019-03-11 (annotated)
- Committer:
- shimizuta
- Date:
- Mon Mar 11 10:38:07 2019 +0000
- Revision:
- 50:36741e8ab197
- Parent:
- 43:2ed84f3558c1
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 27:79b4b932a6dd | 1 | #define _USE_MATH_DEFINES |
shimizuta | 27:79b4b932a6dd | 2 | #include "math.h" |
shimizuta | 11:e81425872740 | 3 | #include "OneLeg.h" |
shimizuta | 14:d7cb429946f4 | 4 | #include "pi.h" |
shimizuta | 27:79b4b932a6dd | 5 | #include "stdio.h" |
shimizuta | 11:e81425872740 | 6 | //注:未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない |
shimizuta | 11:e81425872740 | 7 | float OneLeg::GetX_m() |
shimizuta | 11:e81425872740 | 8 | { |
shimizuta | 11:e81425872740 | 9 | return x_m_; |
shimizuta | 11:e81425872740 | 10 | } |
shimizuta | 11:e81425872740 | 11 | float OneLeg::GetY_m() |
shimizuta | 11:e81425872740 | 12 | { |
shimizuta | 11:e81425872740 | 13 | return y_m_; |
shimizuta | 11:e81425872740 | 14 | } |
shimizuta | 19:1adc7302cfd9 | 15 | OneLeg::OneLeg(float between_servo_half_m, float leglength1[2], float leglength2[2]) |
shimizuta | 11:e81425872740 | 16 | { |
shimizuta | 11:e81425872740 | 17 | rad_[0] = 0; |
shimizuta | 11:e81425872740 | 18 | rad_[1] = M_PI; |
shimizuta | 11:e81425872740 | 19 | between_servo_half_m_ = between_servo_half_m; |
shimizuta | 19:1adc7302cfd9 | 20 | for (int i = 0; i < 2; i++) |
shimizuta | 19:1adc7302cfd9 | 21 | { |
shimizuta | 19:1adc7302cfd9 | 22 | leglength1_[i] = leglength1[i]; |
shimizuta | 19:1adc7302cfd9 | 23 | leglength2_[i] = leglength2[i]; |
shimizuta | 19:1adc7302cfd9 | 24 | } |
shimizuta | 11:e81425872740 | 25 | } |
shimizuta | 27:79b4b932a6dd | 26 | int OneLeg::SetXY_m(float x_m, float y_m) |
shimizuta | 11:e81425872740 | 27 | { |
shimizuta | 27:79b4b932a6dd | 28 | int is_error = 0; |
shimizuta | 19:1adc7302cfd9 | 29 | //対応する角度も同時に計算。 |
shimizuta | 11:e81425872740 | 30 | //処理を軽くするために共通部分は先に計算 |
shimizuta | 11:e81425872740 | 31 | float temp_x[] = { |
shimizuta | 11:e81425872740 | 32 | x_m + between_servo_half_m_, |
shimizuta | 11:e81425872740 | 33 | x_m - between_servo_half_m_, |
shimizuta | 11:e81425872740 | 34 | }; |
shimizuta | 11:e81425872740 | 35 | float temp_y2 = y_m * y_m; |
shimizuta | 19:1adc7302cfd9 | 36 | float temp_L[2]; |
shimizuta | 19:1adc7302cfd9 | 37 | for (int i = 0; i < 2; i++) |
shimizuta | 27:79b4b932a6dd | 38 | temp_L[i] = leglength1_[i] * leglength1_[i] - leglength2_[i] * leglength2_[i]; |
shimizuta | 11:e81425872740 | 39 | |
shimizuta | 11:e81425872740 | 40 | float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); |
shimizuta | 11:e81425872740 | 41 | float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); |
shimizuta | 19:1adc7302cfd9 | 42 | float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L[0] + r1 * r1) / (2.0f * r1 * leglength1_[0])), |
shimizuta | 19:1adc7302cfd9 | 43 | atan2(y_m, temp_x[0]) + acos((temp_L[1] + r2 * r2) / (2.0f * r2 * leglength1_[1]))}; |
shimizuta | 19:1adc7302cfd9 | 44 | if (isnan(targetTheta[0]) || isnan(targetTheta[1])) //解が出ないときは値を更新しない |
shimizuta | 19:1adc7302cfd9 | 45 | { |
shimizuta | 43:2ed84f3558c1 | 46 | printf("error:(x,y) = (%f,%f) is out of range\r\n", x_m, y_m); |
shimizuta | 27:79b4b932a6dd | 47 | is_error = 1; |
shimizuta | 19:1adc7302cfd9 | 48 | } |
shimizuta | 19:1adc7302cfd9 | 49 | else //解が出るならrad, x, y を更新する |
shimizuta | 19:1adc7302cfd9 | 50 | { |
shimizuta | 19:1adc7302cfd9 | 51 | for (size_t i = 0; i < 2; i++) |
shimizuta | 19:1adc7302cfd9 | 52 | SetRad(targetTheta[i], i); |
shimizuta | 19:1adc7302cfd9 | 53 | x_m_ = x_m, y_m_ = y_m; |
shimizuta | 19:1adc7302cfd9 | 54 | } |
shimizuta | 27:79b4b932a6dd | 55 | return is_error; |
shimizuta | 11:e81425872740 | 56 | } |
shimizuta | 11:e81425872740 | 57 | void OneLeg::SetRad(float rad, int servo_num) |
shimizuta | 11:e81425872740 | 58 | { |
shimizuta | 11:e81425872740 | 59 | rad_[servo_num] = rad; |
shimizuta | 11:e81425872740 | 60 | } |
shimizuta | 11:e81425872740 | 61 | float OneLeg::GetRad(int servo_num) |
shimizuta | 11:e81425872740 | 62 | { |
shimizuta | 11:e81425872740 | 63 | return rad_[servo_num]; |
shimizuta | 11:e81425872740 | 64 | } |