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@9:d1fc0805ec7d, 2015-10-14 (annotated)

Committer:
shimogamo
Date:
Wed Oct 14 13:44:32 2015 +0000
Revision:
9:d1fc0805ec7d
Parent:
8:ca92cb674004
Child:
10:0a4bf8c82493
Init??????

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