NagaokaRoboticsClub_mbedTeam / prototype01

s-shape control

Dependents:   wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue

prototype01.cpp@1:ad711893a3a7, 2019-08-22 (annotated)

Committer:
piroro4560
Date:
Thu Aug 22 06:50:42 2019 +0000
Revision:
1:ad711893a3a7
Parent:
0:92d6abcc2b52 
ver_01

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 1:ad711893a3a7 1 #include "proto01.h"
piroro4560 0:92d6abcc2b52 2
piroro4560 0:92d6abcc2b52 3 Sample::Sample(double accdis_, double decdis_, double maxspeed_, double s_vector_)
piroro4560 0:92d6abcc2b52 4 {
piroro4560 0:92d6abcc2b52 5 accdis = accdis_;
piroro4560 0:92d6abcc2b52 6 decdis = decdis_;
piroro4560 0:92d6abcc2b52 7 maxspeed = maxspeed_;
piroro4560 0:92d6abcc2b52 8 s_vector = s_vector_;
piroro4560 0:92d6abcc2b52 9 accdis2 = accdis;
piroro4560 0:92d6abcc2b52 10 decdis2 = decdis;
piroro4560 0:92d6abcc2b52 11 }
piroro4560 0:92d6abcc2b52 12
piroro4560 0:92d6abcc2b52 13 void Sample::target_xy(int targetx_, int targety_, int startx_, int starty_)
piroro4560 0:92d6abcc2b52 14 {
piroro4560 0:92d6abcc2b52 15 targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_));
piroro4560 0:92d6abcc2b52 16 startx = startx_;
piroro4560 0:92d6abcc2b52 17 starty = starty_;
piroro4560 0:92d6abcc2b52 18 targetx = targetx_;
piroro4560 0:92d6abcc2b52 19 targety = targety_;
piroro4560 0:92d6abcc2b52 20 if (targetDis > accdis+decdis) {
piroro4560 0:92d6abcc2b52 21 accdis = accdis2;
piroro4560 0:92d6abcc2b52 22 decdis = decdis2;
piroro4560 0:92d6abcc2b52 23 middledis = targetDis - accdis - decdis;
piroro4560 0:92d6abcc2b52 24 } else {
piroro4560 0:92d6abcc2b52 25 maxspeed /= 2;
piroro4560 0:92d6abcc2b52 26 accdis = targetDis / 2;
piroro4560 0:92d6abcc2b52 27 decdis = targetDis / 2;
piroro4560 0:92d6abcc2b52 28 middledis = 0;
piroro4560 0:92d6abcc2b52 29 }
piroro4560 0:92d6abcc2b52 30 accsec = 2.0 / maxspeed * accdis;
piroro4560 0:92d6abcc2b52 31 decsec = 2.0 / maxspeed * decdis;
piroro4560 0:92d6abcc2b52 32 }
piroro4560 0:92d6abcc2b52 33
piroro4560 0:92d6abcc2b52 34 void Sample::calculate(int now_x, int now_y)
piroro4560 0:92d6abcc2b52 35 {
piroro4560 0:92d6abcc2b52 36 nowx = now_x;
piroro4560 0:92d6abcc2b52 37 nowy = now_y;
piroro4560 0:92d6abcc2b52 38 nowDis = hypot((startx-nowx),(starty-nowy));
piroro4560 0:92d6abcc2b52 39 targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx));
piroro4560 0:92d6abcc2b52 40
piroro4560 0:92d6abcc2b52 41 if (nowDis < accdis) {
piroro4560 0:92d6abcc2b52 42 counter = sqrt(2.0*accsec/maxspeed*nowDis);
piroro4560 0:92d6abcc2b52 43 vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
piroro4560 0:92d6abcc2b52 44 } else if (nowDis >= accdis && nowDis < accdis+middledis) {
piroro4560 0:92d6abcc2b52 45 vector = maxspeed;
piroro4560 0:92d6abcc2b52 46 } else if (nowDis > (accdis+middledis)) {
piroro4560 0:92d6abcc2b52 47 counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis));
piroro4560 0:92d6abcc2b52 48 vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed);
piroro4560 0:92d6abcc2b52 49 } else {
piroro4560 0:92d6abcc2b52 50 vector = 0;
piroro4560 0:92d6abcc2b52 51 }
piroro4560 0:92d6abcc2b52 52 }
piroro4560 0:92d6abcc2b52 53
piroro4560 0:92d6abcc2b52 54 double Sample::getvalue_x()
piroro4560 0:92d6abcc2b52 55 {
piroro4560 0:92d6abcc2b52 56 return vector * cos(targetRad);
piroro4560 0:92d6abcc2b52 57 }
piroro4560 0:92d6abcc2b52 58
piroro4560 0:92d6abcc2b52 59 double Sample::getvalue_y()
piroro4560 0:92d6abcc2b52 60 {
piroro4560 0:92d6abcc2b52 61 return vector * sin(targetRad);
piroro4560 0:92d6abcc2b52 62 }