s-shape control

Dependents:   wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue

Committer:
piroro4560
Date:
Mon Aug 26 06:10:05 2019 +0000
Revision:
4:40c5ac00bb5b
Parent:
3:c46f084010aa
Child:
5:8dce5092a719
It became more useful !!

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_;
piroro4560 4:40c5ac00bb5b 6 accdis2 = accdis;
piroro4560 4:40c5ac00bb5b 7 decdis = decdis_;
piroro4560 4:40c5ac00bb5b 8 decdis2 = decdis;
piroro4560 4:40c5ac00bb5b 9 maxspeed = maxspeed_;
piroro4560 4:40c5ac00bb5b 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 {
piroro4560 4:40c5ac00bb5b 24 maxspeed /= 2;
piroro4560 4:40c5ac00bb5b 25 accdis = targetDis / 2;
piroro4560 4:40c5ac00bb5b 26 decdis = targetDis / 2;
piroro4560 4:40c5ac00bb5b 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 }
piroro4560 0:92d6abcc2b52 33
piroro4560 3:c46f084010aa 34 void Proto1::target_xy(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 {
piroro4560 0:92d6abcc2b52 47 maxspeed /= 2;
piroro4560 0:92d6abcc2b52 48 accdis = targetDis / 2;
piroro4560 0:92d6abcc2b52 49 decdis = targetDis / 2;
piroro4560 4:40c5ac00bb5b 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 }
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
piroro4560 4:40c5ac00bb5b 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 {
piroro4560 0:92d6abcc2b52 73 if (nowDis < accdis) {
piroro4560 0:92d6abcc2b52 74 counter = sqrt(2.0*accsec/maxspeed*nowDis);
piroro4560 0:92d6abcc2b52 75 vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
piroro4560 4:40c5ac00bb5b 76 } else if (nowDis >= accdis && nowDis < accdis+consdis) {
piroro4560 0:92d6abcc2b52 77 vector = maxspeed;
piroro4560 4:40c5ac00bb5b 78 } else if (nowDis > (accdis+consdis)) {
piroro4560 0:92d6abcc2b52 79 counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis));
piroro4560 0:92d6abcc2b52 80 vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed);
piroro4560 0:92d6abcc2b52 81 } else {
piroro4560 0:92d6abcc2b52 82 vector = 0;
piroro4560 0:92d6abcc2b52 83 }
piroro4560 0:92d6abcc2b52 84 }
piroro4560 0:92d6abcc2b52 85
piroro4560 4:40c5ac00bb5b 86 double Proto1::getvalue()
piroro4560 4:40c5ac00bb5b 87 {
piroro4560 4:40c5ac00bb5b 88 return vector;
piroro4560 4:40c5ac00bb5b 89 }
piroro4560 4:40c5ac00bb5b 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 }
piroro4560 0:92d6abcc2b52 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 }