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@8:ca92cb674004, 2015-10-13 (annotated)

Committer:
shimogamo
Date:
Tue Oct 13 13:10:21 2015 +0000
Revision:
8:ca92cb674004
Parent:
7:6f7bd18ce796
Child:
9:d1fc0805ec7d
play????????

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 8:ca92cb674004 8 *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で、trim値を加え、
shimogamo 4:650af94bf062 9 *（ニュートラルが0.5からずれているかもしれない）、サーボの角度に変換してglobalに値をセットする
shimogamo 4:650af94bf062 10 */
shimogamo 0:2a15bd367891 11
shimogamo 5:9a1ec02229dd 12
shimogamo 0:2a15bd367891 13 ControllerManager::ControllerManager(PinName b, PinName h, PinName v)
shimogamo 0:2a15bd367891 14 : controller(b, h, v){
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 8:ca92cb674004 62 Global::setpitchdegree(controller.getV());//生データであることに注意
shimogamo 8:ca92cb674004 63 Global::setyawdegree(controller.getH());//生データであることに注意
shimogamo 8:ca92cb674004 64 // printf("pitch=%f yaw=%f\n", getpitch(), getyaw());
shimogamo 8:ca92cb674004 65 // printf("pitchdeg(raw)=%f yawdeg(raw)=%f\n", getpitchdegree(), getyawdegree());
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 8:ca92cb674004 81 //ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(neutraldegreeの値をとったとき0を返す)
shimogamo 0:2a15bd367891 82 double ControllerManager::pitchratio(){
shimogamo 7:6f7bd18ce796 83 if(controller.getV() < neutralpitchdegree){
shimogamo 7:6f7bd18ce796 84 return clamp((controller.getV() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
shimogamo 7:6f7bd18ce796 85 }else{
shimogamo 7:6f7bd18ce796 86 return clamp((controller.getV() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
shimogamo 7:6f7bd18ce796 87 }
shimogamo 0:2a15bd367891 88 }
shimogamo 0:2a15bd367891 89 double ControllerManager::yawratio(){
shimogamo 7:6f7bd18ce796 90 if(controller.getH() < neutralyawdegree){
shimogamo 7:6f7bd18ce796 91 return clamp((controller.getH() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
shimogamo 7:6f7bd18ce796 92 }else{
shimogamo 7:6f7bd18ce796 93 return clamp((controller.getH() - 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 0:2a15bd367891 150 double ControllerManager::getpitch(){
shimogamo 8:ca92cb674004 151 return clamp(doublepitch(pitchratio())+Global::gettrimpitch(), minpitch, maxpitch);
shimogamo 0:2a15bd367891 152 }
shimogamo 0:2a15bd367891 153 double ControllerManager::getyaw(){
shimogamo 8:ca92cb674004 154 return clamp(doubleyaw(yawratio())+Global::gettrimyaw(), 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 }