NagaokaRoboticsClub_mbedTeam / prototype01

s-shape control

Dependents:   wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue

proto01.cpp@5:8dce5092a719, 2019-09-04 (annotated)

Committer:
ec30109b
Date:
Wed Sep 04 10:01:50 2019 +0000
Revision:
5:8dce5092a719
Parent:
4:40c5ac00bb5b
Child:
6:16a24200ab68
bug

Who changed what in which revision?

UserRevisionLine numberNew 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_;
ec30109b 5:8dce5092a719 6 accdis2 = accdis_;
piroro4560 4:40c5ac00bb5b 7 decdis = decdis_;
ec30109b 5:8dce5092a719 8 decdis2 = decdis_;
piroro4560 4:40c5ac00bb5b 9 maxspeed = maxspeed_;
ec30109b 5:8dce5092a719 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 {
ec30109b 5:8dce5092a719 24 maxspeed = maxspeed2 / 2.0;
ec30109b 5:8dce5092a719 25 accdis = targetDis / 2.0;
ec30109b 5:8dce5092a719 26 decdis = targetDis / 2.0;
ec30109b 5:8dce5092a719 27 consdis = 0.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
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 5:8dce5092a719 47 maxspeed = maxspeed2 / 2.0;
ec30109b 5:8dce5092a719 48 accdis = targetDis / 2.0;
ec30109b 5:8dce5092a719 49 decdis = targetDis / 2.0;
ec30109b 5:8dce5092a719 50 consdis = 0.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
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 }
piroro4560 0:92d6abcc2b52 70
piroro4560 4:40c5ac00bb5b 71 void Proto1::calculate()
piroro4560 4:40c5ac00bb5b 72 {
ec30109b 5:8dce5092a719 73 // if(targetDis <= (accdis+decdis)){
ec30109b 5:8dce5092a719 74 // if(nowDis >= (accdis+decdis)){
ec30109b 5:8dce5092a719 75 // vector = 0;
ec30109b 5:8dce5092a719 76 // }else{
ec30109b 5:8dce5092a719 77 // vector = maxspeed - s_vector;
ec30109b 5:8dce5092a719 78 // }
ec30109b 5:8dce5092a719 79 // }else{
ec30109b 5:8dce5092a719 80 if (nowDis < accdis) {
ec30109b 5:8dce5092a719 81 counter = sqrt(2.0*accsec/maxspeed*fabs(nowDis));
ec30109b 5:8dce5092a719 82 vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
ec30109b 5:8dce5092a719 83 } else if (nowDis >= accdis && nowDis < accdis+consdis) {
ec30109b 5:8dce5092a719 84 vector = maxspeed;
ec30109b 5:8dce5092a719 85 } else if (nowDis > (accdis+consdis)) {
ec30109b 5:8dce5092a719 86 counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis));
ec30109b 5:8dce5092a719 87 vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed);
ec30109b 5:8dce5092a719 88 } else {
ec30109b 5:8dce5092a719 89 vector = 0;
ec30109b 5:8dce5092a719 90 }
ec30109b 5:8dce5092a719 91 // }
piroro4560 0:92d6abcc2b52 92 }
piroro4560 0:92d6abcc2b52 93
piroro4560 4:40c5ac00bb5b 94 double Proto1::getvalue()
piroro4560 4:40c5ac00bb5b 95 {
piroro4560 4:40c5ac00bb5b 96 return vector;
piroro4560 4:40c5ac00bb5b 97 }
piroro4560 4:40c5ac00bb5b 98
piroro4560 3:c46f084010aa 99 double Proto1::getvalue_x()
piroro4560 0:92d6abcc2b52 100 {
piroro4560 0:92d6abcc2b52 101 return vector * cos(targetRad);
piroro4560 0:92d6abcc2b52 102 }
piroro4560 0:92d6abcc2b52 103
piroro4560 3:c46f084010aa 104 double Proto1::getvalue_y()
piroro4560 0:92d6abcc2b52 105 {
piroro4560 0:92d6abcc2b52 106 return vector * sin(targetRad);
piroro4560 0:92d6abcc2b52 107 }