tarou yamada
Dependencies: SimpleMapSerialization serial_communication
communication.cpp@8:52ccf5b84a05, 2016-04-09 (annotated)
- Committer:
- inst
- Date:
- Sat Apr 09 02:09:22 2016 +0000
- Revision:
- 8:52ccf5b84a05
- Parent:
- 7:6c4b22a43eb3
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| inst | 0:8b0edcbd3b87 | 1 | #include "communication.hpp" |
| inst | 0:8b0edcbd3b87 | 2 | #include "moving_object.hpp" |
| inst | 0:8b0edcbd3b87 | 3 | #include "serial_communication.hpp" |
| inst | 0:8b0edcbd3b87 | 4 | |
| inst | 0:8b0edcbd3b87 | 5 | #include "mbed_stl.hpp" |
| inst | 0:8b0edcbd3b87 | 6 | #include "mbed.h" |
| inst | 0:8b0edcbd3b87 | 7 | |
| inst | 0:8b0edcbd3b87 | 8 | communication* communication::instance_ = NULL; |
| inst | 2:074942a78af1 | 9 | const int communication::velocity_offset_ = 32767; |
| inst | 7:6c4b22a43eb3 | 10 | //const float communication::analog_stick_lower_limit_ = 0.2f; |
| inst | 7:6c4b22a43eb3 | 11 | //const size_t communication::velocity_div_num_ = 8; |
| inst | 2:074942a78af1 | 12 | |
| inst | 2:074942a78af1 | 13 | communication::communication() {} |
| inst | 0:8b0edcbd3b87 | 14 | |
| inst | 0:8b0edcbd3b87 | 15 | node_system::moving_object::CONTROL_MODE communication::get_mode() const { |
| inst | 0:8b0edcbd3b87 | 16 | return node_system::moving_object::MANUAL_MODE; |
| inst | 0:8b0edcbd3b87 | 17 | } |
| inst | 0:8b0edcbd3b87 | 18 | |
| inst | 0:8b0edcbd3b87 | 19 | communication::vector2f communication::get_velocity() const { |
| inst | 0:8b0edcbd3b87 | 20 | uint32_t iv[] = { |
| inst | 2:074942a78af1 | 21 | serial_communication::instance()->get("stkLX"), |
| inst | 2:074942a78af1 | 22 | serial_communication::instance()->get("stkLY") |
| inst | 0:8b0edcbd3b87 | 23 | }; |
| inst | 0:8b0edcbd3b87 | 24 | |
| inst | 2:074942a78af1 | 25 | if ((iv[0] == velocity_offset_) && (iv[1] == velocity_offset_)) { |
| inst | 0:8b0edcbd3b87 | 26 | return vector2f(); |
| inst | 0:8b0edcbd3b87 | 27 | } |
| inst | 0:8b0edcbd3b87 | 28 | |
| inst | 0:8b0edcbd3b87 | 29 | vector2f v; |
| inst | 0:8b0edcbd3b87 | 30 | |
| inst | 4:24d383b68566 | 31 | // 受信データは [0, 65535] の範囲であるので [-1, 1] に変換する |
| inst | 0:8b0edcbd3b87 | 32 | for (uint32_t i = 0; i < 2; ++i) { |
| inst | 0:8b0edcbd3b87 | 33 | v[i] = iv[i]; |
| inst | 2:074942a78af1 | 34 | v[i] -= velocity_offset_; |
| inst | 2:074942a78af1 | 35 | v[i] /= velocity_offset_; |
| inst | 0:8b0edcbd3b87 | 36 | } |
| inst | 0:8b0edcbd3b87 | 37 | if (v.length() > 1.0f) { |
| inst | 7:6c4b22a43eb3 | 38 | //v = v.unit(); |
| inst | 2:074942a78af1 | 39 | return v.unit(); |
| inst | 0:8b0edcbd3b87 | 40 | } |
| inst | 7:6c4b22a43eb3 | 41 | return v; |
| inst | 7:6c4b22a43eb3 | 42 | /* |
| inst | 7:6c4b22a43eb3 | 43 | float p = 2.0f * PI;// / 8.0f;// / velocity_div_num_; |
| inst | 7:6c4b22a43eb3 | 44 | float rad = atan2(v.y(), v.x()); |
| inst | 7:6c4b22a43eb3 | 45 | rad += p * 0.5f; |
| inst | 7:6c4b22a43eb3 | 46 | rad /= p; |
| inst | 7:6c4b22a43eb3 | 47 | rad *= p; |
| inst | 7:6c4b22a43eb3 | 48 | */ |
| inst | 7:6c4b22a43eb3 | 49 | //float l = sqrt(v.x() * v.x() + v.y() * v.y()); |
| inst | 7:6c4b22a43eb3 | 50 | |
| inst | 7:6c4b22a43eb3 | 51 | //float l = v.length(); |
| inst | 7:6c4b22a43eb3 | 52 | |
| inst | 7:6c4b22a43eb3 | 53 | //return vector2f(cos(rad), sin(rad)); |
| inst | 7:6c4b22a43eb3 | 54 | //return vector2f(0.3f * cos(rad), 0.3f * sin(rad)); |
| inst | 7:6c4b22a43eb3 | 55 | //return vector2f(l * cos(rad), l * sin(rad)); |
| inst | 0:8b0edcbd3b87 | 56 | |
| inst | 5:b1573848235e | 57 | /* |
| inst | 4:24d383b68566 | 58 | if (v.length() < analog_stick_lower_limit_) { |
| inst | 4:24d383b68566 | 59 | return vector2f(); // しきい値以下なら移動操作無しとみなす |
| inst | 4:24d383b68566 | 60 | } |
| inst | 5:b1573848235e | 61 | */ |
| inst | 7:6c4b22a43eb3 | 62 | //return v; |
| inst | 2:074942a78af1 | 63 | |
| inst | 0:8b0edcbd3b87 | 64 | /* |
| inst | 0:8b0edcbd3b87 | 65 | vector2f velocity = v.unit(); |
| inst | 0:8b0edcbd3b87 | 66 | return velocity; |
| inst | 0:8b0edcbd3b87 | 67 | */ |
| inst | 0:8b0edcbd3b87 | 68 | } |
| inst | 0:8b0edcbd3b87 | 69 | float communication::get_angular_velocity_rad_per_sec() const { |
| inst | 0:8b0edcbd3b87 | 70 | return 0.0f; |
| inst | 0:8b0edcbd3b87 | 71 | } |
| inst | 0:8b0edcbd3b87 | 72 | communication::vector2f communication::get_target_position_mm() const { |
| inst | 0:8b0edcbd3b87 | 73 | return vector2f(); |
| inst | 0:8b0edcbd3b87 | 74 | } |
| inst | 0:8b0edcbd3b87 | 75 | float communication::get_target_heading_rad() const { |
| inst | 0:8b0edcbd3b87 | 76 | return 0.0f; |
| inst | 0:8b0edcbd3b87 | 77 | } |