yashirou shimogamo / Mbed 2 deprecated BIRD2019

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.

Dependencies:   mbed mbed-rtos SPI_MX25R

Control/ControllerManager.cpp@0:d6402bcd58f7, 2018-12-01 (annotated)

Committer:
shimogamo
Date:
Sat Dec 01 15:10:42 2018 +0000
Revision:
0:d6402bcd58f7
BIRD2018????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimogamo 0:d6402bcd58f7 1 #include "mbed.h"
shimogamo 0:d6402bcd58f7 2 #include "Global.h"
shimogamo 0:d6402bcd58f7 3 #include "ControllerManager.h"
shimogamo 0:d6402bcd58f7 4 #include "Trim.h"
shimogamo 0:d6402bcd58f7 5
shimogamo 0:d6402bcd58f7 6 /*
shimogamo 0:d6402bcd58f7 7 *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で
shimogamo 0:d6402bcd58f7 8 *（ニュートラルが0.5からずれているかもしれない）、trim値を加え、
shimogamo 0:d6402bcd58f7 9 *サーボの角度に変換してglobalに値をセットする
shimogamo 0:d6402bcd58f7 10 */
shimogamo 0:d6402bcd58f7 11
shimogamo 0:d6402bcd58f7 12 //コンストラクタでpitch, yawのデフォルトGlobal内から取ってきて、セット
shimogamo 0:d6402bcd58f7 13 ControllerManager::ControllerManager(PinName h, PinName v, PinName hTrimup, PinName hTrimdown)
shimogamo 0:d6402bcd58f7 14 : controllerH(h), controllerV(v), pitchtrim(hTrimup, hTrimdown){
shimogamo 0:d6402bcd58f7 15 maxpitch = Global::getmaxpitch();
shimogamo 0:d6402bcd58f7 16 minpitch = Global::getminpitch();
shimogamo 0:d6402bcd58f7 17 maxyaw = Global::getmaxyaw();
shimogamo 0:d6402bcd58f7 18 minyaw = Global::getminyaw();
shimogamo 0:d6402bcd58f7 19
shimogamo 0:d6402bcd58f7 20 maxpitchdegree=Global::getmaxpitchdegree();
shimogamo 0:d6402bcd58f7 21 neutralpitchdegree=Global::getneutralpitchdegree();
shimogamo 0:d6402bcd58f7 22 minpitchdegree=Global::getminpitchdegree();
shimogamo 0:d6402bcd58f7 23 maxyawdegree=Global::getmaxyawdegree();
shimogamo 0:d6402bcd58f7 24 neutralyawdegree=Global::getneutralyawdegree();
shimogamo 0:d6402bcd58f7 25 minyawdegree=Global::getminyawdegree();
shimogamo 0:d6402bcd58f7 26
shimogamo 0:d6402bcd58f7 27 maxpitchplayratio = Global::getmaxpitchplayratio();
shimogamo 0:d6402bcd58f7 28 minpitchplayratio = Global::getminpitchplayratio();
shimogamo 0:d6402bcd58f7 29 maxyawplayratio = Global::getmaxyawplayratio();
shimogamo 0:d6402bcd58f7 30 minyawplayratio = Global::getminyawplayratio();
shimogamo 0:d6402bcd58f7 31 }
shimogamo 0:d6402bcd58f7 32
shimogamo 0:d6402bcd58f7 33
shimogamo 0:d6402bcd58f7 34 //非線形にする時用．今回は別の関数で非線形？にしている
shimogamo 0:d6402bcd58f7 35 double ControllerManager::calc(double doublex,int intx){
shimogamo 0:d6402bcd58f7 36 return doublex;
shimogamo 0:d6402bcd58f7 37 // return intx+pow(doublex-intx,3);
shimogamo 0:d6402bcd58f7 38 }
shimogamo 0:d6402bcd58f7 39
shimogamo 0:d6402bcd58f7 40
shimogamo 0:d6402bcd58f7 41
shimogamo 0:d6402bcd58f7 42
shimogamo 0:d6402bcd58f7 43 void ControllerManager::update(){
shimogamo 0:d6402bcd58f7 44 //updateのたびにめったに変化しないmax,min,neutralの値をglobalから読み込むの非効率な気がする。もっといいやりかたがあるはず...
shimogamo 0:d6402bcd58f7 45 maxpitch = Global::getmaxpitch();
shimogamo 0:d6402bcd58f7 46 minpitch = Global::getminpitch();
shimogamo 0:d6402bcd58f7 47 maxyaw = Global::getmaxyaw();
shimogamo 0:d6402bcd58f7 48 minyaw = Global::getminyaw();
shimogamo 0:d6402bcd58f7 49 maxpitchdegree=Global::getmaxpitchdegree();
shimogamo 0:d6402bcd58f7 50 neutralpitchdegree=Global::getneutralpitchdegree();
shimogamo 0:d6402bcd58f7 51 minpitchdegree=Global::getminpitchdegree();
shimogamo 0:d6402bcd58f7 52 maxyawdegree=Global::getmaxyawdegree();
shimogamo 0:d6402bcd58f7 53 neutralyawdegree=Global::getneutralyawdegree();
shimogamo 0:d6402bcd58f7 54 minyawdegree=Global::getminyawdegree();
shimogamo 0:d6402bcd58f7 55 maxpitchplayratio = Global::getmaxpitchplayratio();
shimogamo 0:d6402bcd58f7 56 minpitchplayratio = Global::getminpitchplayratio();
shimogamo 0:d6402bcd58f7 57 maxyawplayratio = Global::getmaxyawplayratio();
shimogamo 0:d6402bcd58f7 58 minyawplayratio = Global::getminyawplayratio();
shimogamo 0:d6402bcd58f7 59
shimogamo 0:d6402bcd58f7 60
shimogamo 0:d6402bcd58f7 61 Global::setpitch(getpitch());
shimogamo 0:d6402bcd58f7 62 Global::setyaw(getyaw());
shimogamo 0:d6402bcd58f7 63 Global::setpitchdegree(controllerV.read());//生データであることに注意
shimogamo 0:d6402bcd58f7 64 Global::setyawdegree(controllerH.read());//生データであることに注意
shimogamo 0:d6402bcd58f7 65 Global::setinttrimpitch(pitchtrim.getint());
shimogamo 0:d6402bcd58f7 66 }
shimogamo 0:d6402bcd58f7 67
shimogamo 0:d6402bcd58f7 68
shimogamo 0:d6402bcd58f7 69
shimogamo 0:d6402bcd58f7 70
shimogamo 0:d6402bcd58f7 71 //設定した最大、最小値を超えない値を返す
shimogamo 0:d6402bcd58f7 72 static float clamp(double value, double min, double max) {
shimogamo 0:d6402bcd58f7 73 if(value < min) {
shimogamo 0:d6402bcd58f7 74 return min;
shimogamo 0:d6402bcd58f7 75 } else if(value > max) {
shimogamo 0:d6402bcd58f7 76 return max;
shimogamo 0:d6402bcd58f7 77 } else {
shimogamo 0:d6402bcd58f7 78 return value;
shimogamo 0:d6402bcd58f7 79 }
shimogamo 0:d6402bcd58f7 80 }
shimogamo 0:d6402bcd58f7 81
shimogamo 0:d6402bcd58f7 82 //ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(neutraldegreeの値をとったとき0を返す)
shimogamo 0:d6402bcd58f7 83 //式中のパラメータは全てAnalogIn,[0, 1]の範囲であることに注意
shimogamo 0:d6402bcd58f7 84 double ControllerManager::pitchratio(){
shimogamo 0:d6402bcd58f7 85 if(controllerV.read() < neutralpitchdegree){
shimogamo 0:d6402bcd58f7 86 return clamp((controllerV.read() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
shimogamo 0:d6402bcd58f7 87 }else{
shimogamo 0:d6402bcd58f7 88 return clamp((controllerV.read() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
shimogamo 0:d6402bcd58f7 89 }
shimogamo 0:d6402bcd58f7 90 }
shimogamo 0:d6402bcd58f7 91 double ControllerManager::yawratio(){
shimogamo 0:d6402bcd58f7 92 if(controllerH.read() < neutralyawdegree){
shimogamo 0:d6402bcd58f7 93 return clamp((controllerH.read() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
shimogamo 0:d6402bcd58f7 94 }else{
shimogamo 0:d6402bcd58f7 95 return clamp((controllerH.read() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
shimogamo 0:d6402bcd58f7 96 }
shimogamo 0:d6402bcd58f7 97 }
shimogamo 0:d6402bcd58f7 98
shimogamo 0:d6402bcd58f7 99 //pitchratioに遊びの追加
shimogamo 0:d6402bcd58f7 100 //wikiに詳細が載っている
shimogamo 0:d6402bcd58f7 101 double ControllerManager::pitchratioplayed(double pitchratio){
shimogamo 0:d6402bcd58f7 102 if(pitchratio > maxpitchplayratio){
shimogamo 0:d6402bcd58f7 103 return (pitchratio - maxpitchplayratio) / (1.0 - maxpitchplayratio);
shimogamo 0:d6402bcd58f7 104 }else if(pitchratio < minpitchplayratio){
shimogamo 0:d6402bcd58f7 105 return (pitchratio - minpitchplayratio) / (minpitchplayratio + 1.0);
shimogamo 0:d6402bcd58f7 106 }else{
shimogamo 0:d6402bcd58f7 107 return 0;
shimogamo 0:d6402bcd58f7 108 }
shimogamo 0:d6402bcd58f7 109 }
shimogamo 0:d6402bcd58f7 110 //pitchratioplayedと同様
shimogamo 0:d6402bcd58f7 111 double ControllerManager::yawratioplayed(double yawratio){
shimogamo 0:d6402bcd58f7 112 if(yawratio > maxyawplayratio){
shimogamo 0:d6402bcd58f7 113 return (yawratio - maxyawplayratio) / (1.0 - maxyawplayratio);
shimogamo 0:d6402bcd58f7 114 }else if(yawratio < minyawplayratio){
shimogamo 0:d6402bcd58f7 115 return (yawratio - minyawplayratio) / (minyawplayratio + 1.0);
shimogamo 0:d6402bcd58f7 116 }else{
shimogamo 0:d6402bcd58f7 117 return 0;
shimogamo 0:d6402bcd58f7 118 }
shimogamo 0:d6402bcd58f7 119 }
shimogamo 0:d6402bcd58f7 120
shimogamo 0:d6402bcd58f7 121
shimogamo 0:d6402bcd58f7 122 //pitchratioを設定したmaxpitch,minpitch倍にする（ニュートラル付近の値の加工もここで行う）
shimogamo 0:d6402bcd58f7 123 double ControllerManager::doublepitch(double pitchratio){
shimogamo 0:d6402bcd58f7 124 if(pitchratio<0){
shimogamo 0:d6402bcd58f7 125 return -pitchratioplayed(pitchratio)*minpitch;
shimogamo 0:d6402bcd58f7 126 }else{
shimogamo 0:d6402bcd58f7 127 return pitchratioplayed(pitchratio)*maxpitch;
shimogamo 0:d6402bcd58f7 128 }
shimogamo 0:d6402bcd58f7 129 }
shimogamo 0:d6402bcd58f7 130 double ControllerManager::doubleyaw(double yawratio){
shimogamo 0:d6402bcd58f7 131 if(yawratio<0){
shimogamo 0:d6402bcd58f7 132 return -yawratioplayed(yawratio)*minyaw;
shimogamo 0:d6402bcd58f7 133 }else{
shimogamo 0:d6402bcd58f7 134 return yawratioplayed(yawratio)*maxyaw;
shimogamo 0:d6402bcd58f7 135 }
shimogamo 0:d6402bcd58f7 136 }
shimogamo 0:d6402bcd58f7 137 int ControllerManager::intpitch(double pitchratio){
shimogamo 0:d6402bcd58f7 138 if(pitchratio<0){
shimogamo 0:d6402bcd58f7 139 return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
shimogamo 0:d6402bcd58f7 140 }else{
shimogamo 0:d6402bcd58f7 141 return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
shimogamo 0:d6402bcd58f7 142 }
shimogamo 0:d6402bcd58f7 143 }
shimogamo 0:d6402bcd58f7 144 int ControllerManager::intyaw(double yawratio){
shimogamo 0:d6402bcd58f7 145 if(yawratio<0){
shimogamo 0:d6402bcd58f7 146 return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
shimogamo 0:d6402bcd58f7 147 }else{
shimogamo 0:d6402bcd58f7 148 return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
shimogamo 0:d6402bcd58f7 149 }
shimogamo 0:d6402bcd58f7 150 }
shimogamo 0:d6402bcd58f7 151
shimogamo 0:d6402bcd58f7 152
shimogamo 0:d6402bcd58f7 153
shimogamo 0:d6402bcd58f7 154 //ここでtrim値を加える
shimogamo 0:d6402bcd58f7 155 double ControllerManager::getpitch(){//trim値を加えた結果maxminを超えないようにclamp
shimogamo 0:d6402bcd58f7 156 return clamp(doublepitch(pitchratio())+pitchtrim.get(Global::gettrimpitchrate()), minpitch, maxpitch);
shimogamo 0:d6402bcd58f7 157 }
shimogamo 0:d6402bcd58f7 158 double ControllerManager::getyaw(){
shimogamo 0:d6402bcd58f7 159 return clamp(doubleyaw(yawratio()), minyaw, maxyaw);
shimogamo 0:d6402bcd58f7 160 }
shimogamo 0:d6402bcd58f7 161
shimogamo 0:d6402bcd58f7 162 //おそらくデバッグ用(pitch, yawが正しく計算されているか、確認)
shimogamo 0:d6402bcd58f7 163 double ControllerManager::getpitch(double _pitchratio){
shimogamo 0:d6402bcd58f7 164 return doublepitch(_pitchratio);
shimogamo 0:d6402bcd58f7 165 }
shimogamo 0:d6402bcd58f7 166 double ControllerManager::getyaw(double _yawratio){
shimogamo 0:d6402bcd58f7 167 return doubleyaw(_yawratio);
shimogamo 0:d6402bcd58f7 168 }