s-shape control
proto01.cpp@5:b257821e4713, 2019-09-27 (annotated)
- Committer:
- piroro4560
- Date:
- Fri Sep 27 10:49:35 2019 +0000
- Revision:
- 5:b257821e4713
- Parent:
- 4:40c5ac00bb5b
adjustment
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 1:ad711893a3a7 | 1 | #include "proto01.h" |
piroro4560 | 0:92d6abcc2b52 | 2 | |
piroro4560 | 3:c46f084010aa | 3 | Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_) |
piroro4560 | 0:92d6abcc2b52 | 4 | { |
piroro4560 | 4:40c5ac00bb5b | 5 | accdis = accdis_; |
piroro4560 | 4:40c5ac00bb5b | 6 | accdis2 = accdis; |
piroro4560 | 4:40c5ac00bb5b | 7 | decdis = decdis_; |
piroro4560 | 4:40c5ac00bb5b | 8 | decdis2 = decdis; |
piroro4560 | 4:40c5ac00bb5b | 9 | maxspeed = maxspeed_; |
piroro4560 | 4:40c5ac00bb5b | 10 | maxspeed2 = maxspeed; |
piroro4560 | 4:40c5ac00bb5b | 11 | s_vector = s_vector_; |
piroro4560 | 4:40c5ac00bb5b | 12 | } |
piroro4560 | 4:40c5ac00bb5b | 13 | |
piroro4560 | 4:40c5ac00bb5b | 14 | void Proto1::target(double target_, double start_) |
piroro4560 | 4:40c5ac00bb5b | 15 | { |
piroro4560 | 4:40c5ac00bb5b | 16 | targetDis = target_; |
piroro4560 | 4:40c5ac00bb5b | 17 | start = start_; |
piroro4560 | 4:40c5ac00bb5b | 18 | if (targetDis > accdis+decdis) { |
piroro4560 | 4:40c5ac00bb5b | 19 | maxspeed = maxspeed2; |
piroro4560 | 4:40c5ac00bb5b | 20 | accdis = accdis2; |
piroro4560 | 4:40c5ac00bb5b | 21 | decdis = decdis2; |
piroro4560 | 4:40c5ac00bb5b | 22 | consdis = targetDis - accdis - decdis; |
piroro4560 | 4:40c5ac00bb5b | 23 | } else { |
piroro4560 | 5:b257821e4713 | 24 | maxspeed = maxspeed2 / 2.0; |
piroro4560 | 4:40c5ac00bb5b | 25 | accdis = targetDis / 2; |
piroro4560 | 4:40c5ac00bb5b | 26 | decdis = targetDis / 2; |
piroro4560 | 4:40c5ac00bb5b | 27 | consdis = 0; |
piroro4560 | 4:40c5ac00bb5b | 28 | } |
piroro4560 | 4:40c5ac00bb5b | 29 | /*秘伝のコード*/ /*いじるな*/ |
piroro4560 | 4:40c5ac00bb5b | 30 | accsec = 2.0 / maxspeed * accdis; |
piroro4560 | 4:40c5ac00bb5b | 31 | decsec = 2.0 / maxspeed * decdis; |
piroro4560 | 0:92d6abcc2b52 | 32 | } |
piroro4560 | 0:92d6abcc2b52 | 33 | |
piroro4560 | 5:b257821e4713 | 34 | void Proto1::targetXY(int targetx_, int targety_, int startx_, int starty_) |
piroro4560 | 0:92d6abcc2b52 | 35 | { |
piroro4560 | 0:92d6abcc2b52 | 36 | targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_)); |
piroro4560 | 4:40c5ac00bb5b | 37 | startx = startx_; |
piroro4560 | 4:40c5ac00bb5b | 38 | starty = starty_; |
piroro4560 | 4:40c5ac00bb5b | 39 | targetx = targetx_; |
piroro4560 | 4:40c5ac00bb5b | 40 | targety = targety_; |
piroro4560 | 0:92d6abcc2b52 | 41 | if (targetDis > accdis+decdis) { |
piroro4560 | 4:40c5ac00bb5b | 42 | maxspeed = maxspeed2; |
piroro4560 | 0:92d6abcc2b52 | 43 | accdis = accdis2; |
piroro4560 | 0:92d6abcc2b52 | 44 | decdis = decdis2; |
piroro4560 | 4:40c5ac00bb5b | 45 | consdis = targetDis - accdis - decdis; |
piroro4560 | 0:92d6abcc2b52 | 46 | } else { |
piroro4560 | 5:b257821e4713 | 47 | maxspeed = maxspeed / 2.0; |
piroro4560 | 0:92d6abcc2b52 | 48 | accdis = targetDis / 2; |
piroro4560 | 0:92d6abcc2b52 | 49 | decdis = targetDis / 2; |
piroro4560 | 4:40c5ac00bb5b | 50 | consdis = 0; |
piroro4560 | 0:92d6abcc2b52 | 51 | } |
piroro4560 | 4:40c5ac00bb5b | 52 | /*秘伝のコード*/ /*いじるな*/ |
piroro4560 | 0:92d6abcc2b52 | 53 | accsec = 2.0 / maxspeed * accdis; |
piroro4560 | 0:92d6abcc2b52 | 54 | decsec = 2.0 / maxspeed * decdis; |
piroro4560 | 0:92d6abcc2b52 | 55 | } |
piroro4560 | 0:92d6abcc2b52 | 56 | |
piroro4560 | 4:40c5ac00bb5b | 57 | void Proto1::Input_now(double now_) |
piroro4560 | 4:40c5ac00bb5b | 58 | { |
piroro4560 | 4:40c5ac00bb5b | 59 | now = now_; |
piroro4560 | 4:40c5ac00bb5b | 60 | nowDis = now - start; |
piroro4560 | 4:40c5ac00bb5b | 61 | } |
piroro4560 | 4:40c5ac00bb5b | 62 | |
piroro4560 | 5:b257821e4713 | 63 | void Proto1::Input_nowXY(int now_x, int now_y) |
piroro4560 | 0:92d6abcc2b52 | 64 | { |
piroro4560 | 4:40c5ac00bb5b | 65 | nowx = now_x; |
piroro4560 | 4:40c5ac00bb5b | 66 | nowy = now_y; |
piroro4560 | 4:40c5ac00bb5b | 67 | nowDis = hypot((startx-nowx),(starty-nowy)); |
piroro4560 | 0:92d6abcc2b52 | 68 | targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx)); |
piroro4560 | 4:40c5ac00bb5b | 69 | } |
piroro4560 | 0:92d6abcc2b52 | 70 | |
piroro4560 | 4:40c5ac00bb5b | 71 | void Proto1::calculate() |
piroro4560 | 4:40c5ac00bb5b | 72 | { |
piroro4560 | 0:92d6abcc2b52 | 73 | if (nowDis < accdis) { |
piroro4560 | 0:92d6abcc2b52 | 74 | counter = sqrt(2.0*accsec/maxspeed*nowDis); |
piroro4560 | 0:92d6abcc2b52 | 75 | vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector)); |
piroro4560 | 4:40c5ac00bb5b | 76 | } else if (nowDis >= accdis && nowDis < accdis+consdis) { |
piroro4560 | 0:92d6abcc2b52 | 77 | vector = maxspeed; |
piroro4560 | 4:40c5ac00bb5b | 78 | } else if (nowDis > (accdis+consdis)) { |
piroro4560 | 0:92d6abcc2b52 | 79 | counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis)); |
piroro4560 | 0:92d6abcc2b52 | 80 | vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed); |
piroro4560 | 0:92d6abcc2b52 | 81 | } else { |
piroro4560 | 0:92d6abcc2b52 | 82 | vector = 0; |
piroro4560 | 0:92d6abcc2b52 | 83 | } |
piroro4560 | 0:92d6abcc2b52 | 84 | } |
piroro4560 | 0:92d6abcc2b52 | 85 | |
piroro4560 | 4:40c5ac00bb5b | 86 | double Proto1::getvalue() |
piroro4560 | 4:40c5ac00bb5b | 87 | { |
piroro4560 | 4:40c5ac00bb5b | 88 | return vector; |
piroro4560 | 4:40c5ac00bb5b | 89 | } |
piroro4560 | 4:40c5ac00bb5b | 90 | |
piroro4560 | 3:c46f084010aa | 91 | double Proto1::getvalue_x() |
piroro4560 | 0:92d6abcc2b52 | 92 | { |
piroro4560 | 0:92d6abcc2b52 | 93 | return vector * cos(targetRad); |
piroro4560 | 0:92d6abcc2b52 | 94 | } |
piroro4560 | 0:92d6abcc2b52 | 95 | |
piroro4560 | 3:c46f084010aa | 96 | double Proto1::getvalue_y() |
piroro4560 | 0:92d6abcc2b52 | 97 | { |
piroro4560 | 0:92d6abcc2b52 | 98 | return vector * sin(targetRad); |
piroro4560 | 0:92d6abcc2b52 | 99 | } |