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@20:6fbf1f50abda, 2016-05-30 (annotated)

Committer:
shimogamo
Date:
Mon May 30 07:19:34 2016 +0000
Revision:
20:6fbf1f50abda
Parent:
12:8e39bb45c61c
Child:
26:a53c3208ac35
Servo??????????, Joystick???

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 5:9a1ec02229dd 44 maxpitch = Global::getmaxpitch();
shimogamo 5:9a1ec02229dd 45 minpitch = Global::getminpitch();
shimogamo 5:9a1ec02229dd 46 maxyaw = Global::getmaxyaw();
shimogamo 5:9a1ec02229dd 47 minyaw = Global::getminyaw();
shimogamo 5:9a1ec02229dd 48 maxpitchdegree=Global::getmaxpitchdegree();
shimogamo 5:9a1ec02229dd 49 neutralpitchdegree=Global::getneutralpitchdegree();
shimogamo 5:9a1ec02229dd 50 minpitchdegree=Global::getminpitchdegree();
shimogamo 5:9a1ec02229dd 51 maxyawdegree=Global::getmaxyawdegree();
shimogamo 5:9a1ec02229dd 52 neutralyawdegree=Global::getneutralyawdegree();
shimogamo 5:9a1ec02229dd 53 minyawdegree=Global::getminyawdegree();
shimogamo 8:ca92cb674004 54 maxpitchplayratio = Global::getmaxpitchplayratio();
shimogamo 8:ca92cb674004 55 minpitchplayratio = Global::getminpitchplayratio();
shimogamo 8:ca92cb674004 56 maxyawplayratio = Global::getmaxyawplayratio();
shimogamo 8:ca92cb674004 57 minyawplayratio = Global::getminyawplayratio();
shimogamo 8:ca92cb674004 58
shimogamo 5:9a1ec02229dd 59
shimogamo 4:650af94bf062 60 Global::setpitch(getpitch());
shimogamo 4:650af94bf062 61 Global::setyaw(getyaw());
shimogamo 20:6fbf1f50abda 62 Global::setpitchdegree(controllerV.read());//生データであることに注意
shimogamo 20:6fbf1f50abda 63 Global::setyawdegree(controllerH.read());//生データであることに注意
shimogamo 12:8e39bb45c61c 64 Global::setinttrimpitch(pitchtrim.getint());
shimogamo 0:2a15bd367891 65 }
shimogamo 0:2a15bd367891 66
shimogamo 0:2a15bd367891 67
shimogamo 0:2a15bd367891 68
shimogamo 0:2a15bd367891 69
shimogamo 2:e0f1e8662b8c 70 //設定した最大、最小値を超えない値を返す
shimogamo 4:650af94bf062 71 static float clamp(double value, double min, double max) {
shimogamo 0:2a15bd367891 72 if(value < min) {
shimogamo 0:2a15bd367891 73 return min;
shimogamo 0:2a15bd367891 74 } else if(value > max) {
shimogamo 0:2a15bd367891 75 return max;
shimogamo 0:2a15bd367891 76 } else {
shimogamo 0:2a15bd367891 77 return value;
shimogamo 0:2a15bd367891 78 }
shimogamo 0:2a15bd367891 79 }
shimogamo 20:6fbf1f50abda 80
shimogamo 8:ca92cb674004 81 //ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(neutraldegreeの値をとったとき0を返す)
shimogamo 0:2a15bd367891 82 double ControllerManager::pitchratio(){
shimogamo 20:6fbf1f50abda 83 if(controllerV.read() < neutralpitchdegree){
shimogamo 20:6fbf1f50abda 84 return clamp((controllerV.read() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
shimogamo 7:6f7bd18ce796 85 }else{
shimogamo 20:6fbf1f50abda 86 return clamp((controllerV.read() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
shimogamo 7:6f7bd18ce796 87 }
shimogamo 0:2a15bd367891 88 }
shimogamo 0:2a15bd367891 89 double ControllerManager::yawratio(){
shimogamo 20:6fbf1f50abda 90 if(controllerH.read() < neutralyawdegree){
shimogamo 20:6fbf1f50abda 91 return clamp((controllerH.read() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
shimogamo 7:6f7bd18ce796 92 }else{
shimogamo 20:6fbf1f50abda 93 return clamp((controllerH.read() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
shimogamo 7:6f7bd18ce796 94 }
shimogamo 0:2a15bd367891 95 }
shimogamo 0:2a15bd367891 96
shimogamo 8:ca92cb674004 97 double ControllerManager::pitchratioplayed(double pitchratio){
shimogamo 8:ca92cb674004 98 if(pitchratio > maxpitchplayratio){
shimogamo 8:ca92cb674004 99 return (pitchratio - maxpitchplayratio) / (1.0 - maxpitchplayratio);
shimogamo 8:ca92cb674004 100 }else if(pitchratio < minpitchplayratio){
shimogamo 8:ca92cb674004 101 return (pitchratio - minpitchplayratio) / (minpitchplayratio + 1.0);
shimogamo 8:ca92cb674004 102 }else{
shimogamo 8:ca92cb674004 103 return 0;
shimogamo 8:ca92cb674004 104 }
shimogamo 8:ca92cb674004 105 }
shimogamo 8:ca92cb674004 106 double ControllerManager::yawratioplayed(double yawratio){
shimogamo 8:ca92cb674004 107 if(yawratio > maxyawplayratio){
shimogamo 8:ca92cb674004 108 return (yawratio - maxyawplayratio) / (1.0 - maxyawplayratio);
shimogamo 8:ca92cb674004 109 }else if(yawratio < minyawplayratio){
shimogamo 8:ca92cb674004 110 return (yawratio - minyawplayratio) / (minyawplayratio + 1.0);
shimogamo 8:ca92cb674004 111 }else{
shimogamo 8:ca92cb674004 112 return 0;
shimogamo 8:ca92cb674004 113 }
shimogamo 8:ca92cb674004 114 }
shimogamo 0:2a15bd367891 115
shimogamo 8:ca92cb674004 116
shimogamo 8:ca92cb674004 117 //pitchratioを設定したmaxpitch,minpitch倍にする（ニュートラル付近の値の加工もここで行う）
shimogamo 0:2a15bd367891 118 double ControllerManager::doublepitch(double pitchratio){
shimogamo 0:2a15bd367891 119 if(pitchratio<0){
shimogamo 8:ca92cb674004 120 return -pitchratioplayed(pitchratio)*minpitch;
shimogamo 0:2a15bd367891 121 }else{
shimogamo 8:ca92cb674004 122 return pitchratioplayed(pitchratio)*maxpitch;
shimogamo 0:2a15bd367891 123 }
shimogamo 0:2a15bd367891 124 }
shimogamo 0:2a15bd367891 125 double ControllerManager::doubleyaw(double yawratio){
shimogamo 0:2a15bd367891 126 if(yawratio<0){
shimogamo 8:ca92cb674004 127 return -yawratioplayed(yawratio)*minyaw;
shimogamo 0:2a15bd367891 128 }else{
shimogamo 8:ca92cb674004 129 return yawratioplayed(yawratio)*maxyaw;
shimogamo 0:2a15bd367891 130 }
shimogamo 0:2a15bd367891 131 }
shimogamo 0:2a15bd367891 132 int ControllerManager::intpitch(double pitchratio){
shimogamo 0:2a15bd367891 133 if(pitchratio<0){
shimogamo 0:2a15bd367891 134 return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 135 }else{
shimogamo 0:2a15bd367891 136 return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 137 }
shimogamo 0:2a15bd367891 138 }
shimogamo 0:2a15bd367891 139 int ControllerManager::intyaw(double yawratio){
shimogamo 0:2a15bd367891 140 if(yawratio<0){
shimogamo 0:2a15bd367891 141 return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 142 }else{
shimogamo 0:2a15bd367891 143 return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 144 }
shimogamo 0:2a15bd367891 145 }
shimogamo 0:2a15bd367891 146
shimogamo 0:2a15bd367891 147
shimogamo 0:2a15bd367891 148
shimogamo 8:ca92cb674004 149 //ここでtrim値を加える
shimogamo 20:6fbf1f50abda 150 double ControllerManager::getpitch(){//trim値を加えた結果maxminを超えないようにclamp
shimogamo 12:8e39bb45c61c 151 return clamp(doublepitch(pitchratio())+pitchtrim.get(Global::gettrimpitchrate()), minpitch, maxpitch);
shimogamo 0:2a15bd367891 152 }
shimogamo 0:2a15bd367891 153 double ControllerManager::getyaw(){
shimogamo 12:8e39bb45c61c 154 return clamp(doubleyaw(yawratio()), minyaw, maxyaw);
shimogamo 0:2a15bd367891 155 }
shimogamo 8:ca92cb674004 156
shimogamo 0:2a15bd367891 157 double ControllerManager::getpitch(double _pitchratio){
shimogamo 8:ca92cb674004 158 return doublepitch(_pitchratio);
shimogamo 0:2a15bd367891 159 }
shimogamo 0:2a15bd367891 160 double ControllerManager::getyaw(double _yawratio){
shimogamo 8:ca92cb674004 161 return doubleyaw(_yawratio);
shimogamo 0:2a15bd367891 162 }