sample
sample02.cpp@3:8d1778dbf580, 2019-07-11 (annotated)
- Committer:
- piroro4560
- Date:
- Thu Jul 11 08:25:18 2019 +0000
- Revision:
- 3:8d1778dbf580
- Parent:
- 2:b51bb221438a
- Child:
- 4:3eefc20c8312
- Child:
- 6:1e1adccfb69a
***
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:0b1d25a1682d | 1 | #include "sample02.h" |
piroro4560 | 0:0b1d25a1682d | 2 | |
piroro4560 | 2:b51bb221438a | 3 | Sample::Sample(double accdis_, double decdis_, double maxspeed_, double s_vector_) |
piroro4560 | 0:0b1d25a1682d | 4 | { |
piroro4560 | 2:b51bb221438a | 5 | accdis = accdis_; |
piroro4560 | 2:b51bb221438a | 6 | decdis = decdis_; |
piroro4560 | 2:b51bb221438a | 7 | maxspeed = maxspeed_; |
piroro4560 | 2:b51bb221438a | 8 | s_vector = s_vector_; |
piroro4560 | 0:0b1d25a1682d | 9 | } |
piroro4560 | 0:0b1d25a1682d | 10 | |
piroro4560 | 2:b51bb221438a | 11 | void Sample::target_xy(int targetx_, int targety_, int startx_, int starty_) |
piroro4560 | 0:0b1d25a1682d | 12 | { |
piroro4560 | 2:b51bb221438a | 13 | targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_)); |
piroro4560 | 2:b51bb221438a | 14 | if (targetDis > accdis+decdis) { |
piroro4560 | 0:0b1d25a1682d | 15 | startx = startx_; |
piroro4560 | 0:0b1d25a1682d | 16 | starty = starty_; |
piroro4560 | 2:b51bb221438a | 17 | targetx = targetx_; |
piroro4560 | 2:b51bb221438a | 18 | targety = targety_; |
piroro4560 | 0:0b1d25a1682d | 19 | accsec = 2.0 / maxspeed * accdis; |
piroro4560 | 0:0b1d25a1682d | 20 | decsec = 2.0 / maxspeed * decdis; |
piroro4560 | 2:b51bb221438a | 21 | middledis = targetDis - accdis - decdis; |
piroro4560 | 0:0b1d25a1682d | 22 | } |
piroro4560 | 0:0b1d25a1682d | 23 | } |
piroro4560 | 0:0b1d25a1682d | 24 | |
piroro4560 | 0:0b1d25a1682d | 25 | void Sample::calculate(int now_x, int now_y) |
piroro4560 | 0:0b1d25a1682d | 26 | { |
piroro4560 | 0:0b1d25a1682d | 27 | nowx = now_x; |
piroro4560 | 0:0b1d25a1682d | 28 | nowy = now_y; |
piroro4560 | 1:85da508f8d7e | 29 | nowDis = hypot((startx-nowx),(starty-nowy)); |
piroro4560 | 2:b51bb221438a | 30 | targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx)); |
piroro4560 | 0:0b1d25a1682d | 31 | /***/ |
piroro4560 | 0:0b1d25a1682d | 32 | if (nowDis < accdis) { |
piroro4560 | 1:85da508f8d7e | 33 | |
piroro4560 | 0:0b1d25a1682d | 34 | counter = sqrt(2.0*accsec/maxspeed*nowDis); |
piroro4560 | 1:85da508f8d7e | 35 | vector = s_vector + (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector)); |
piroro4560 | 1:85da508f8d7e | 36 | |
piroro4560 | 1:85da508f8d7e | 37 | } else if (nowDis >= accdis && nowDis < accdis+middledis) { |
piroro4560 | 0:0b1d25a1682d | 38 | vector = maxspeed; |
piroro4560 | 3:8d1778dbf580 | 39 | } else if (nowDis > (accdis+middledis) && nowDis <= targetDis) { |
piroro4560 | 1:85da508f8d7e | 40 | |
piroro4560 | 3:8d1778dbf580 | 41 | counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis)); |
piroro4560 | 3:8d1778dbf580 | 42 | vector = (-1.0 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed)*0.9; |
piroro4560 | 0:0b1d25a1682d | 43 | } |
piroro4560 | 0:0b1d25a1682d | 44 | /***/ |
piroro4560 | 0:0b1d25a1682d | 45 | } |
piroro4560 | 0:0b1d25a1682d | 46 | |
piroro4560 | 0:0b1d25a1682d | 47 | double Sample::getvalue_x() |
piroro4560 | 0:0b1d25a1682d | 48 | { |
piroro4560 | 2:b51bb221438a | 49 | return vector * cos(targetRad); |
piroro4560 | 0:0b1d25a1682d | 50 | } |
piroro4560 | 0:0b1d25a1682d | 51 | |
piroro4560 | 0:0b1d25a1682d | 52 | double Sample::getvalue_y() |
piroro4560 | 0:0b1d25a1682d | 53 | { |
piroro4560 | 2:b51bb221438a | 54 | return vector * sin(targetRad); |
piroro4560 | 0:0b1d25a1682d | 55 | } |