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@10:0a4bf8c82493, 2015-11-26 (annotated)

Committer:
shimogamo
Date:
Thu Nov 26 08:14:41 2015 +0000
Revision:
10:0a4bf8c82493
Parent:
9:d1fc0805ec7d
Child:
12:8e39bb45c61c
previous pin assign

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 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 8:ca92cb674004 80 //ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(neutraldegreeの値をとったとき0を返す)
shimogamo 0:2a15bd367891 81 double ControllerManager::pitchratio(){
shimogamo 7:6f7bd18ce796 82 if(controller.getV() < neutralpitchdegree){
shimogamo 7:6f7bd18ce796 83 return clamp((controller.getV() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
shimogamo 7:6f7bd18ce796 84 }else{
shimogamo 7:6f7bd18ce796 85 return clamp((controller.getV() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
shimogamo 7:6f7bd18ce796 86 }
shimogamo 0:2a15bd367891 87 }
shimogamo 0:2a15bd367891 88 double ControllerManager::yawratio(){
shimogamo 7:6f7bd18ce796 89 if(controller.getH() < neutralyawdegree){
shimogamo 7:6f7bd18ce796 90 return clamp((controller.getH() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
shimogamo 7:6f7bd18ce796 91 }else{
shimogamo 7:6f7bd18ce796 92 return clamp((controller.getH() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
shimogamo 7:6f7bd18ce796 93 }
shimogamo 0:2a15bd367891 94 }
shimogamo 0:2a15bd367891 95
shimogamo 8:ca92cb674004 96 double ControllerManager::pitchratioplayed(double pitchratio){
shimogamo 8:ca92cb674004 97 if(pitchratio > maxpitchplayratio){
shimogamo 8:ca92cb674004 98 return (pitchratio - maxpitchplayratio) / (1.0 - maxpitchplayratio);
shimogamo 8:ca92cb674004 99 }else if(pitchratio < minpitchplayratio){
shimogamo 8:ca92cb674004 100 return (pitchratio - minpitchplayratio) / (minpitchplayratio + 1.0);
shimogamo 8:ca92cb674004 101 }else{
shimogamo 8:ca92cb674004 102 return 0;
shimogamo 8:ca92cb674004 103 }
shimogamo 8:ca92cb674004 104 }
shimogamo 8:ca92cb674004 105 double ControllerManager::yawratioplayed(double yawratio){
shimogamo 8:ca92cb674004 106 if(yawratio > maxyawplayratio){
shimogamo 8:ca92cb674004 107 return (yawratio - maxyawplayratio) / (1.0 - maxyawplayratio);
shimogamo 8:ca92cb674004 108 }else if(yawratio < minyawplayratio){
shimogamo 8:ca92cb674004 109 return (yawratio - minyawplayratio) / (minyawplayratio + 1.0);
shimogamo 8:ca92cb674004 110 }else{
shimogamo 8:ca92cb674004 111 return 0;
shimogamo 8:ca92cb674004 112 }
shimogamo 8:ca92cb674004 113 }
shimogamo 0:2a15bd367891 114
shimogamo 8:ca92cb674004 115
shimogamo 8:ca92cb674004 116 //pitchratioを設定したmaxpitch,minpitch倍にする（ニュートラル付近の値の加工もここで行う）
shimogamo 0:2a15bd367891 117 double ControllerManager::doublepitch(double pitchratio){
shimogamo 0:2a15bd367891 118 if(pitchratio<0){
shimogamo 8:ca92cb674004 119 return -pitchratioplayed(pitchratio)*minpitch;
shimogamo 0:2a15bd367891 120 }else{
shimogamo 8:ca92cb674004 121 return pitchratioplayed(pitchratio)*maxpitch;
shimogamo 0:2a15bd367891 122 }
shimogamo 0:2a15bd367891 123 }
shimogamo 0:2a15bd367891 124 double ControllerManager::doubleyaw(double yawratio){
shimogamo 0:2a15bd367891 125 if(yawratio<0){
shimogamo 8:ca92cb674004 126 return -yawratioplayed(yawratio)*minyaw;
shimogamo 0:2a15bd367891 127 }else{
shimogamo 8:ca92cb674004 128 return yawratioplayed(yawratio)*maxyaw;
shimogamo 0:2a15bd367891 129 }
shimogamo 0:2a15bd367891 130 }
shimogamo 0:2a15bd367891 131 int ControllerManager::intpitch(double pitchratio){
shimogamo 0:2a15bd367891 132 if(pitchratio<0){
shimogamo 0:2a15bd367891 133 return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 134 }else{
shimogamo 0:2a15bd367891 135 return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 136 }
shimogamo 0:2a15bd367891 137 }
shimogamo 0:2a15bd367891 138 int ControllerManager::intyaw(double yawratio){
shimogamo 0:2a15bd367891 139 if(yawratio<0){
shimogamo 0:2a15bd367891 140 return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 141 }else{
shimogamo 0:2a15bd367891 142 return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 143 }
shimogamo 0:2a15bd367891 144 }
shimogamo 0:2a15bd367891 145
shimogamo 0:2a15bd367891 146
shimogamo 0:2a15bd367891 147
shimogamo 8:ca92cb674004 148 //ここでtrim値を加える
shimogamo 0:2a15bd367891 149 double ControllerManager::getpitch(){
shimogamo 10:0a4bf8c82493 150 //printf("pitchratio=%f doublepitch=%f gettrimpitch=%f\n", pitchratio(), doublepitch(pitchratio()), Global::gettrimpitch());
shimogamo 8:ca92cb674004 151 return clamp(doublepitch(pitchratio())+Global::gettrimpitch(), minpitch, maxpitch);
shimogamo 0:2a15bd367891 152 }
shimogamo 0:2a15bd367891 153 double ControllerManager::getyaw(){
shimogamo 10:0a4bf8c82493 154 //printf("yawratio=%f doubleyaw=%f gettrimyaw=%f\n", yawratio(), doubleyaw(yawratio()), Global::gettrimyaw());
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 }