Ryu Anzawa / sample

sample

Dependents:   wheel_test6

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?

UserRevisionLine numberNew 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 }