AutoControl.cpp

00001 #include "AutoControl.h"
00002 
00003 coords gPosi;
00004 
00005 PathTracking motion(FOLLOW_TANGENT); // 経路追従(接線方向向く)モードでとりあえず初期化
00006 
00007 AutoControl::AutoControl(){
00008 }
00009 
00010 // SDのデータを読み込んで，PathTrackingの変数に格納
00011 int AutoControl::init(mySDclass* mySD, int FieldColor){
00012     return mySD->path_read(FieldColor, motion.Px, motion.Py, motion.refvel, motion.refangle, motion.acc_mode, motion.acc_count, motion.dec_tbe);
00013 }
00014 
00015 void AutoControl::gPosiInit(){
00016     gPosi.x = motion.Px[0];
00017     gPosi.y = motion.Py[0];
00018     gPosi.z = motion.refangle[0];
00019 }
00020 
00021 coords AutoControl::pathTrackingMode(int mode, int state, int nextPhase){ // 軌道追従モード
00022     coords refV;
00023     int pathNum = getPathNum();
00024 
00025     if(motion.getMode() != mode) motion.setMode(mode);
00026     int syusoku = motion.calcRefvel(); // 収束していれば　1　が返ってくる
00027     
00028     if(syusoku == 1){ // 収束して次の曲線へ
00029         if( pathNum <= state ){
00030             motion.Px[3*pathNum+3] = gPosi.x;
00031             motion.Py[3*pathNum+3] = gPosi.y;
00032             motion.incrPathnum(0.02, 0.997); // 次の曲線へ．括弧の中身は収束に使う数値
00033 
00034             if( pathNum == state ) phase = nextPhase;
00035         }
00036     }else if(syusoku == 0){ // まだ収束していない，軌道追従中
00037         refV.x = motion.refVx;
00038         refV.y = motion.refVy;
00039         refV.z = motion.refVz;
00040     }else{ // それ以外は問題ありなので止める
00041         refV.x = 0.0;
00042         refV.y = 0.0;
00043         refV.z = 0.0;
00044     }
00045     return refV;
00046 }
00047 
00048 void AutoControl::calibrationGposi(double tempX, double tempY, double tempZ){
00049     gPosi.x = tempX;
00050     gPosi.y = tempY;
00051     gPosi.z = tempZ;
00052 }
00053 
00054 coords AutoControl::commandMode_vel(double tempX, double tempY, double tempZ){
00055     coords refV;
00056     refV.x = tempX;
00057     refV.y = tempY;
00058     refV.z = tempZ;
00059     return refV;
00060 }
00061 
00062 void AutoControl::commandMode(int nextPhase, bool next/*=true*/){ // 指定した速度で動かすとき
00063     int pathNum = getPathNum();
00064 
00065     if( next ){ // この動きを一つの曲線とみなす場合
00066         motion.Px[3*pathNum+3] = gPosi.x;
00067         motion.Py[3*pathNum+3] = gPosi.y;
00068         motion.incrPathnum(0.02, 0.997); // 次の曲線へ．括弧の中身は収束に使う数値
00069     }else{
00070         motion.Px[3*pathNum] = gPosi.x;
00071         motion.Py[3*pathNum] = gPosi.y;
00072     }
00073     
00074     phase = nextPhase;
00075 }
00076 
00077 int AutoControl::getPathNum(){
00078     return motion.getPathNum();
00079 }
00080 
00081 double AutoControl::onx(){
00082     return motion.onx;
00083 }
00084 
00085 double AutoControl::ony(){
00086     return motion.ony;
00087 }
00088 
00089 double AutoControl::angle(){
00090     return motion.angle;
00091 }
00092 
00093 double AutoControl::dist(){
00094     return motion.dist;
00095 }
00096 
00097 double AutoControl::refKakudo(){
00098     return motion.refKakudo;
00099 }
00100 
00101 void AutoControl::initSettings(){
00102     motion.initSettings();
00103 }
00104 void AutoControl::setConvPara(double conv_length, double conv_tnum){
00105     motion.setConvPara(conv_length, conv_tnum);
00106 }
00107 void AutoControl::setMaxPathnum(int pathNum){
00108     motion.setMaxPathnum(pathNum);
00109 }
00110 
00111 // このメソッドの中身はユーザーが書き換える必要あり
00112 coords AutoControl::getRefVel(unsigned int swState){
00113     coords refV = {0.0, 0.0, 0.0};
00114     static unsigned int pre_swState = swState;
00115 
00116     // Edit here >>>>>
00117     if(0){
00118 
00119     }
00120     else{
00121         // とりあえず常に停止するようにしているので，適宜修正してください
00122         refV = commandMode_vel(0.0, 0.0, 0.0); // 該当しない場合はとりあえず速度ゼロ
00123     }
00124     // <<<<<
00125 
00126     pre_swState = swState;
00127 
00128     return refV;
00129 }