kubtss / Mbed 2 deprecated BIRD2017

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-rtos mbed

Control/ControllerManager.cpp@26:a53c3208ac35, 2016-07-23 (annotated)

Committer:
shimogamo
Date:
Sat Jul 23 00:50:47 2016 +0000
Revision:
26:a53c3208ac35
Parent:
20:6fbf1f50abda 
??OB?????????

Who changed what in which revision?

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