![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
6足歩行試験用プログラム Movement のdefine の値を変えると方向変換
Movement.cpp@0:6b57aea7ad9b, 2021-12-06 (annotated)
- Committer:
- minanao
- Date:
- Mon Dec 06 08:54:49 2021 +0000
- Revision:
- 0:6b57aea7ad9b
6legs_test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
minanao | 0:6b57aea7ad9b | 1 | #include "mbed.h" |
minanao | 0:6b57aea7ad9b | 2 | #include "Servo.h" |
minanao | 0:6b57aea7ad9b | 3 | #include "Movement.h" |
minanao | 0:6b57aea7ad9b | 4 | |
minanao | 0:6b57aea7ad9b | 5 | //上から順にservo1.2.3.4.5.6の基準値 これを変えればすべての値が変わる |
minanao | 0:6b57aea7ad9b | 6 | #define S1 1.0 |
minanao | 0:6b57aea7ad9b | 7 | #define S2 1.0 |
minanao | 0:6b57aea7ad9b | 8 | #define S3 1.0 |
minanao | 0:6b57aea7ad9b | 9 | #define S4 1.0 |
minanao | 0:6b57aea7ad9b | 10 | #define S5 1.0 |
minanao | 0:6b57aea7ad9b | 11 | #define S6 1.0 |
minanao | 0:6b57aea7ad9b | 12 | |
minanao | 0:6b57aea7ad9b | 13 | |
minanao | 0:6b57aea7ad9b | 14 | //前と後ろ |
minanao | 0:6b57aea7ad9b | 15 | Servo servo1(D7); |
minanao | 0:6b57aea7ad9b | 16 | Servo servo3(A1); |
minanao | 0:6b57aea7ad9b | 17 | Servo servo4(D12); |
minanao | 0:6b57aea7ad9b | 18 | Servo servo6(A5); |
minanao | 0:6b57aea7ad9b | 19 | |
minanao | 0:6b57aea7ad9b | 20 | //新しく取り付けるもの |
minanao | 0:6b57aea7ad9b | 21 | Servo servo2(A3); |
minanao | 0:6b57aea7ad9b | 22 | Servo servo5(D10); |
minanao | 0:6b57aea7ad9b | 23 | |
minanao | 0:6b57aea7ad9b | 24 | void Movement::stop(){ |
minanao | 0:6b57aea7ad9b | 25 | servo1 = S1/2; |
minanao | 0:6b57aea7ad9b | 26 | servo3 = S3/2; |
minanao | 0:6b57aea7ad9b | 27 | servo4 = S4/2; |
minanao | 0:6b57aea7ad9b | 28 | servo6 = S6/2; |
minanao | 0:6b57aea7ad9b | 29 | |
minanao | 0:6b57aea7ad9b | 30 | // |
minanao | 0:6b57aea7ad9b | 31 | servo2 = S2/2; |
minanao | 0:6b57aea7ad9b | 32 | servo5 = S5/2; |
minanao | 0:6b57aea7ad9b | 33 | wait(1); |
minanao | 0:6b57aea7ad9b | 34 | } |
minanao | 0:6b57aea7ad9b | 35 | |
minanao | 0:6b57aea7ad9b | 36 | //前進 |
minanao | 0:6b57aea7ad9b | 37 | void Movement::move_forward(int time = 20) |
minanao | 0:6b57aea7ad9b | 38 | { |
minanao | 0:6b57aea7ad9b | 39 | servo1 = 0; |
minanao | 0:6b57aea7ad9b | 40 | servo2 = 0; |
minanao | 0:6b57aea7ad9b | 41 | servo3 = 0; |
minanao | 0:6b57aea7ad9b | 42 | servo4 = 0; |
minanao | 0:6b57aea7ad9b | 43 | servo5 = 0; |
minanao | 0:6b57aea7ad9b | 44 | servo6 = 0; |
minanao | 0:6b57aea7ad9b | 45 | wait(time); |
minanao | 0:6b57aea7ad9b | 46 | } |
minanao | 0:6b57aea7ad9b | 47 | |
minanao | 0:6b57aea7ad9b | 48 | //後退 |
minanao | 0:6b57aea7ad9b | 49 | void Movement::move_backward() |
minanao | 0:6b57aea7ad9b | 50 | { |
minanao | 0:6b57aea7ad9b | 51 | servo1 = S1; |
minanao | 0:6b57aea7ad9b | 52 | servo2 = S2; |
minanao | 0:6b57aea7ad9b | 53 | servo3 = S3; |
minanao | 0:6b57aea7ad9b | 54 | servo4 = S4; |
minanao | 0:6b57aea7ad9b | 55 | servo5 = S5; |
minanao | 0:6b57aea7ad9b | 56 | servo6 = S6; |
minanao | 0:6b57aea7ad9b | 57 | wait(5); |
minanao | 0:6b57aea7ad9b | 58 | } |
minanao | 0:6b57aea7ad9b | 59 | |
minanao | 0:6b57aea7ad9b | 60 | //右に曲がる |
minanao | 0:6b57aea7ad9b | 61 | void Movement::turn_right(int theta = 15) |
minanao | 0:6b57aea7ad9b | 62 | { |
minanao | 0:6b57aea7ad9b | 63 | servo1 = S1; |
minanao | 0:6b57aea7ad9b | 64 | servo2 = S2; |
minanao | 0:6b57aea7ad9b | 65 | servo3 = S3; |
minanao | 0:6b57aea7ad9b | 66 | servo4 = 0; |
minanao | 0:6b57aea7ad9b | 67 | servo5 = 0; |
minanao | 0:6b57aea7ad9b | 68 | servo6 = 0; |
minanao | 0:6b57aea7ad9b | 69 | wait(int(theta/15)); |
minanao | 0:6b57aea7ad9b | 70 | } |
minanao | 0:6b57aea7ad9b | 71 | |
minanao | 0:6b57aea7ad9b | 72 | //左に曲がる |
minanao | 0:6b57aea7ad9b | 73 | void Movement::turn_left(int theta = 15) |
minanao | 0:6b57aea7ad9b | 74 | { |
minanao | 0:6b57aea7ad9b | 75 | servo1 = 0; |
minanao | 0:6b57aea7ad9b | 76 | servo2 = 0; |
minanao | 0:6b57aea7ad9b | 77 | servo3 = 0; |
minanao | 0:6b57aea7ad9b | 78 | servo4 = S4; |
minanao | 0:6b57aea7ad9b | 79 | servo5 = S5; |
minanao | 0:6b57aea7ad9b | 80 | servo6 = S6; |
minanao | 0:6b57aea7ad9b | 81 | wait(int(theta/15)); |
minanao | 0:6b57aea7ad9b | 82 | } |
minanao | 0:6b57aea7ad9b | 83 | |
minanao | 0:6b57aea7ad9b | 84 | //倒れているときの処理 |
minanao | 0:6b57aea7ad9b | 85 | void Movement::wakeup(int time){ |
minanao | 0:6b57aea7ad9b | 86 | for(int i=1;i<=time;i++) |
minanao | 0:6b57aea7ad9b | 87 | { |
minanao | 0:6b57aea7ad9b | 88 | move_forward(5); |
minanao | 0:6b57aea7ad9b | 89 | move_backward(); |
minanao | 0:6b57aea7ad9b | 90 | turn_right(); |
minanao | 0:6b57aea7ad9b | 91 | turn_left(); |
minanao | 0:6b57aea7ad9b | 92 | } |
minanao | 0:6b57aea7ad9b | 93 | } |