NagaokaRoboticsClub_mbedTeam / prototype01

s-shape control

Dependents:   wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue

proto01.cpp@9:4a72ba990137, 2019-10-04 (annotated)

Committer:
ec30109b
Date:
Fri Oct 04 08:21:13 2019 +0000
Revision:
9:4a72ba990137
Parent:
7:c9707baaf007 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 1:ad711893a3a7 1 #include "proto01.h"
ec30109b 9:4a72ba990137 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_;
ec30109b 9:4a72ba990137 6 accdis2 = accdis;
piroro4560 4:40c5ac00bb5b 7 decdis = decdis_;
ec30109b 9:4a72ba990137 8 decdis2 = decdis;
piroro4560 4:40c5ac00bb5b 9 maxspeed = maxspeed_;
ec30109b 9:4a72ba990137 10 maxspeed2 = maxspeed;
piroro4560 4:40c5ac00bb5b 11 s_vector = s_vector_;
piroro4560 4:40c5ac00bb5b 12 }
ec30109b 9:4a72ba990137 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 {
ec30109b 5:8dce5092a719 24 maxspeed = maxspeed2 / 2.0;
ec30109b 9:4a72ba990137 25 accdis = targetDis / 2;
ec30109b 9:4a72ba990137 26 decdis = targetDis / 2;
ec30109b 9:4a72ba990137 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 }
ec30109b 9:4a72ba990137 33
ec30109b 5:8dce5092a719 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 {
ec30109b 9:4a72ba990137 47 maxspeed = maxspeed / 2.0;
ec30109b 9:4a72ba990137 48 accdis = targetDis / 2;
ec30109b 9:4a72ba990137 49 decdis = targetDis / 2;
ec30109b 9:4a72ba990137 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 }
ec30109b 9:4a72ba990137 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 }
ec30109b 9:4a72ba990137 62
ec30109b 5:8dce5092a719 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 }
ec30109b 9:4a72ba990137 70
piroro4560 4:40c5ac00bb5b 71 void Proto1::calculate()
piroro4560 4:40c5ac00bb5b 72 {
ec30109b 6:16a24200ab68 73 if (nowDis < accdis) {
ec30109b 9:4a72ba990137 74 counter = sqrt(2.0*accsec/maxspeed*nowDis);
ec30109b 6:16a24200ab68 75 vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
ec30109b 6:16a24200ab68 76 } else if (nowDis >= accdis && nowDis < accdis+consdis) {
ec30109b 6:16a24200ab68 77 vector = maxspeed;
ec30109b 6:16a24200ab68 78 } else if (nowDis > (accdis+consdis)) {
ec30109b 6:16a24200ab68 79 counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis));
ec30109b 6:16a24200ab68 80 vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed);
ec30109b 6:16a24200ab68 81 } else {
ec30109b 6:16a24200ab68 82 vector = 0;
ec30109b 6:16a24200ab68 83 }
piroro4560 0:92d6abcc2b52 84 }
ec30109b 9:4a72ba990137 85
piroro4560 4:40c5ac00bb5b 86 double Proto1::getvalue()
piroro4560 4:40c5ac00bb5b 87 {
piroro4560 4:40c5ac00bb5b 88 return vector;
piroro4560 4:40c5ac00bb5b 89 }
ec30109b 9:4a72ba990137 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 }
ec30109b 9:4a72ba990137 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 }