s-shape control

Dependents:   wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_blue

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers proto01.cpp Source File

proto01.cpp

00001 #include "proto01.h"
00002  
00003 Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_)
00004 {
00005     accdis    = accdis_;
00006     accdis2   = accdis;
00007     decdis    = decdis_;
00008     decdis2   = decdis;
00009     maxspeed  = maxspeed_;
00010     maxspeed2 = maxspeed;
00011     s_vector  = s_vector_;
00012 }
00013  
00014 void Proto1::target(double target_, double start_)
00015 {
00016     targetDis = target_;
00017     start     = start_;
00018     if (targetDis > accdis+decdis) {
00019         maxspeed  = maxspeed2;
00020         accdis    = accdis2;
00021         decdis    = decdis2;
00022         consdis   = targetDis - accdis - decdis;
00023     } else {
00024         maxspeed  = maxspeed2 / 2.0;
00025         accdis    = targetDis / 2;
00026         decdis    = targetDis / 2;
00027         consdis   = 0;
00028     }
00029     /*秘伝のコード*/ /*いじるな*/
00030     accsec = 2.0 / maxspeed * accdis;
00031     decsec = 2.0 / maxspeed * decdis;
00032 }
00033  
00034 void Proto1::targetXY(int targetx_, int targety_, int startx_, int starty_)
00035 {
00036     targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_));
00037     startx    = startx_;
00038     starty    = starty_;
00039     targetx   = targetx_;
00040     targety   = targety_;
00041     if (targetDis > accdis+decdis) {
00042         maxspeed  = maxspeed2;
00043         accdis    = accdis2;
00044         decdis    = decdis2;
00045         consdis   = targetDis - accdis - decdis;
00046     } else {
00047         maxspeed =  maxspeed / 2.0;
00048         accdis    = targetDis / 2;
00049         decdis    = targetDis / 2;
00050         consdis   = 0;
00051     }
00052     /*秘伝のコード*/ /*いじるな*/
00053     accsec = 2.0 / maxspeed * accdis;
00054     decsec = 2.0 / maxspeed * decdis;
00055 }
00056  
00057 void Proto1::Input_now(double now_)
00058 {
00059     now    = now_;
00060     nowDis = now - start;
00061 }
00062  
00063 void Proto1::Input_nowXY(int now_x, int now_y)
00064 {
00065     nowx      = now_x;
00066     nowy      = now_y;
00067     nowDis    = hypot((startx-nowx),(starty-nowy));
00068     targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx));
00069 }
00070  
00071 void Proto1::calculate()
00072 {
00073     if (nowDis < accdis) {
00074         counter = sqrt(2.0*accsec/maxspeed*nowDis);
00075         vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
00076     } else if (nowDis >= accdis && nowDis < accdis+consdis) {
00077         vector = maxspeed;
00078     } else if (nowDis > (accdis+consdis)) {
00079         counter = sqrt(2.0*decsec/maxspeed*fabs(targetDis-nowDis));
00080         vector = (-1 * sin((2.0*PI/decsec)*counter) + (2.0*PI/decsec)*counter) / (2.0*PI/maxspeed);
00081     } else {
00082         vector = 0;
00083     }
00084 }
00085  
00086 double Proto1::getvalue()
00087 {
00088     return vector;
00089 }
00090  
00091 double Proto1::getvalue_x()
00092 {
00093     return vector * cos(targetRad);
00094 }
00095  
00096 double Proto1::getvalue_y()
00097 {
00098     return vector * sin(targetRad);
00099 }