Kobayashi Akihiro / ActiveCaster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ActiveCaster_ ActiveCaster_2

AutoControl.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 #include "AutoControl.h"
e5119053f6 0:5e4f1e288e2a 2
e5119053f6 0:5e4f1e288e2a 3 coords gPosi;
e5119053f6 0:5e4f1e288e2a 4
e5119053f6 0:5e4f1e288e2a 5 PathTracking motion(FOLLOW_TANGENT); // 経路追従(接線方向向く)モードでとりあえず初期化
e5119053f6 0:5e4f1e288e2a 6
e5119053f6 0:5e4f1e288e2a 7 AutoControl::AutoControl(){
e5119053f6 0:5e4f1e288e2a 8 }
e5119053f6 0:5e4f1e288e2a 9
e5119053f6 0:5e4f1e288e2a 10 // SDのデータを読み込んで，PathTrackingの変数に格納
e5119053f6 0:5e4f1e288e2a 11 int AutoControl::init(mySDclass* mySD, int FieldColor){
e5119053f6 0:5e4f1e288e2a 12 return mySD->path_read(FieldColor, motion.Px, motion.Py, motion.refvel, motion.refangle, motion.acc_mode, motion.acc_count, motion.dec_tbe);
e5119053f6 0:5e4f1e288e2a 13 }
e5119053f6 0:5e4f1e288e2a 14
e5119053f6 0:5e4f1e288e2a 15 void AutoControl::gPosiInit(){
e5119053f6 0:5e4f1e288e2a 16 gPosi.x = motion.Px[0];
e5119053f6 0:5e4f1e288e2a 17 gPosi.y = motion.Py[0];
e5119053f6 0:5e4f1e288e2a 18 gPosi.z = motion.refangle[0];
e5119053f6 0:5e4f1e288e2a 19 }
e5119053f6 0:5e4f1e288e2a 20
e5119053f6 0:5e4f1e288e2a 21 coords AutoControl::pathTrackingMode(int mode, int state, int nextPhase){ // 軌道追従モード
e5119053f6 0:5e4f1e288e2a 22 coords refV;
e5119053f6 0:5e4f1e288e2a 23 int pathNum = getPathNum();
e5119053f6 0:5e4f1e288e2a 24
e5119053f6 0:5e4f1e288e2a 25 if(motion.getMode() != mode) motion.setMode(mode);
e5119053f6 0:5e4f1e288e2a 26 int syusoku = motion.calcRefvel(); // 収束していれば　1　が返ってくる
e5119053f6 0:5e4f1e288e2a 27
e5119053f6 0:5e4f1e288e2a 28 if(syusoku == 1){ // 収束して次の曲線へ
e5119053f6 0:5e4f1e288e2a 29 if( pathNum <= state ){
e5119053f6 0:5e4f1e288e2a 30 motion.Px[3*pathNum+3] = gPosi.x;
e5119053f6 0:5e4f1e288e2a 31 motion.Py[3*pathNum+3] = gPosi.y;
e5119053f6 0:5e4f1e288e2a 32 motion.incrPathnum(0.02, 0.997); // 次の曲線へ．括弧の中身は収束に使う数値
e5119053f6 0:5e4f1e288e2a 33
e5119053f6 0:5e4f1e288e2a 34 if( pathNum == state ) phase = nextPhase;
e5119053f6 0:5e4f1e288e2a 35 }
e5119053f6 0:5e4f1e288e2a 36 }else if(syusoku == 0){ // まだ収束していない，軌道追従中
e5119053f6 0:5e4f1e288e2a 37 refV.x = motion.refVx;
e5119053f6 0:5e4f1e288e2a 38 refV.y = motion.refVy;
e5119053f6 0:5e4f1e288e2a 39 refV.z = motion.refVz;
e5119053f6 0:5e4f1e288e2a 40 }else{ // それ以外は問題ありなので止める
e5119053f6 0:5e4f1e288e2a 41 refV.x = 0.0;
e5119053f6 0:5e4f1e288e2a 42 refV.y = 0.0;
e5119053f6 0:5e4f1e288e2a 43 refV.z = 0.0;
e5119053f6 0:5e4f1e288e2a 44 }
e5119053f6 0:5e4f1e288e2a 45 return refV;
e5119053f6 0:5e4f1e288e2a 46 }
e5119053f6 0:5e4f1e288e2a 47
e5119053f6 0:5e4f1e288e2a 48 void AutoControl::calibrationGposi(double tempX, double tempY, double tempZ){
e5119053f6 0:5e4f1e288e2a 49 gPosi.x = tempX;
e5119053f6 0:5e4f1e288e2a 50 gPosi.y = tempY;
e5119053f6 0:5e4f1e288e2a 51 gPosi.z = tempZ;
e5119053f6 0:5e4f1e288e2a 52 }
e5119053f6 0:5e4f1e288e2a 53
e5119053f6 0:5e4f1e288e2a 54 coords AutoControl::commandMode_vel(double tempX, double tempY, double tempZ){
e5119053f6 0:5e4f1e288e2a 55 coords refV;
e5119053f6 0:5e4f1e288e2a 56 refV.x = tempX;
e5119053f6 0:5e4f1e288e2a 57 refV.y = tempY;
e5119053f6 0:5e4f1e288e2a 58 refV.z = tempZ;
e5119053f6 0:5e4f1e288e2a 59 return refV;
e5119053f6 0:5e4f1e288e2a 60 }
e5119053f6 0:5e4f1e288e2a 61
e5119053f6 0:5e4f1e288e2a 62 void AutoControl::commandMode(int nextPhase, bool next/*=true*/){ // 指定した速度で動かすとき
e5119053f6 0:5e4f1e288e2a 63 int pathNum = getPathNum();
e5119053f6 0:5e4f1e288e2a 64
e5119053f6 0:5e4f1e288e2a 65 if( next ){ // この動きを一つの曲線とみなす場合
e5119053f6 0:5e4f1e288e2a 66 motion.Px[3*pathNum+3] = gPosi.x;
e5119053f6 0:5e4f1e288e2a 67 motion.Py[3*pathNum+3] = gPosi.y;
e5119053f6 0:5e4f1e288e2a 68 motion.incrPathnum(0.02, 0.997); // 次の曲線へ．括弧の中身は収束に使う数値
e5119053f6 0:5e4f1e288e2a 69 }else{
e5119053f6 0:5e4f1e288e2a 70 motion.Px[3*pathNum] = gPosi.x;
e5119053f6 0:5e4f1e288e2a 71 motion.Py[3*pathNum] = gPosi.y;
e5119053f6 0:5e4f1e288e2a 72 }
e5119053f6 0:5e4f1e288e2a 73
e5119053f6 0:5e4f1e288e2a 74 phase = nextPhase;
e5119053f6 0:5e4f1e288e2a 75 }
e5119053f6 0:5e4f1e288e2a 76
e5119053f6 0:5e4f1e288e2a 77 int AutoControl::getPathNum(){
e5119053f6 0:5e4f1e288e2a 78 return motion.getPathNum();
e5119053f6 0:5e4f1e288e2a 79 }
e5119053f6 0:5e4f1e288e2a 80
e5119053f6 0:5e4f1e288e2a 81 double AutoControl::onx(){
e5119053f6 0:5e4f1e288e2a 82 return motion.onx;
e5119053f6 0:5e4f1e288e2a 83 }
e5119053f6 0:5e4f1e288e2a 84
e5119053f6 0:5e4f1e288e2a 85 double AutoControl::ony(){
e5119053f6 0:5e4f1e288e2a 86 return motion.ony;
e5119053f6 0:5e4f1e288e2a 87 }
e5119053f6 0:5e4f1e288e2a 88
e5119053f6 0:5e4f1e288e2a 89 double AutoControl::angle(){
e5119053f6 0:5e4f1e288e2a 90 return motion.angle;
e5119053f6 0:5e4f1e288e2a 91 }
e5119053f6 0:5e4f1e288e2a 92
e5119053f6 0:5e4f1e288e2a 93 double AutoControl::dist(){
e5119053f6 0:5e4f1e288e2a 94 return motion.dist;
e5119053f6 0:5e4f1e288e2a 95 }
e5119053f6 0:5e4f1e288e2a 96
e5119053f6 0:5e4f1e288e2a 97 double AutoControl::refKakudo(){
e5119053f6 0:5e4f1e288e2a 98 return motion.refKakudo;
e5119053f6 0:5e4f1e288e2a 99 }
e5119053f6 0:5e4f1e288e2a 100
e5119053f6 0:5e4f1e288e2a 101 void AutoControl::initSettings(){
e5119053f6 0:5e4f1e288e2a 102 motion.initSettings();
e5119053f6 0:5e4f1e288e2a 103 }
e5119053f6 0:5e4f1e288e2a 104 void AutoControl::setConvPara(double conv_length, double conv_tnum){
e5119053f6 0:5e4f1e288e2a 105 motion.setConvPara(conv_length, conv_tnum);
e5119053f6 0:5e4f1e288e2a 106 }
e5119053f6 0:5e4f1e288e2a 107 void AutoControl::setMaxPathnum(int pathNum){
e5119053f6 0:5e4f1e288e2a 108 motion.setMaxPathnum(pathNum);
e5119053f6 0:5e4f1e288e2a 109 }
e5119053f6 0:5e4f1e288e2a 110
e5119053f6 0:5e4f1e288e2a 111 // このメソッドの中身はユーザーが書き換える必要あり
e5119053f6 0:5e4f1e288e2a 112 coords AutoControl::getRefVel(unsigned int swState){
e5119053f6 0:5e4f1e288e2a 113 coords refV = {0.0, 0.0, 0.0};
e5119053f6 0:5e4f1e288e2a 114 static unsigned int pre_swState = swState;
e5119053f6 0:5e4f1e288e2a 115
e5119053f6 0:5e4f1e288e2a 116 // Edit here >>>>>
e5119053f6 0:5e4f1e288e2a 117 if(0){
e5119053f6 0:5e4f1e288e2a 118
e5119053f6 0:5e4f1e288e2a 119 }
e5119053f6 0:5e4f1e288e2a 120 else{
e5119053f6 0:5e4f1e288e2a 121 // とりあえず常に停止するようにしているので，適宜修正してください
e5119053f6 0:5e4f1e288e2a 122 refV = commandMode_vel(0.0, 0.0, 0.0); // 該当しない場合はとりあえず速度ゼロ
e5119053f6 0:5e4f1e288e2a 123 }
e5119053f6 0:5e4f1e288e2a 124 // <<<<<
e5119053f6 0:5e4f1e288e2a 125
e5119053f6 0:5e4f1e288e2a 126 pre_swState = swState;
e5119053f6 0:5e4f1e288e2a 127
e5119053f6 0:5e4f1e288e2a 128 return refV;
e5119053f6 0:5e4f1e288e2a 129 }