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@7:6f7bd18ce796, 2015-10-10 (annotated)

Committer:
shimogamo
Date:
Sat Oct 10 22:53:05 2015 +0000
Revision:
7:6f7bd18ce796
Parent:
6:0d9fa7152934
Child:
8:ca92cb674004
controllerManager???

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 4:650af94bf062 8 *このクラスでは、AnalogInで受け取った[0,1]の値をジョイスティックのニュートラル補正した上で
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 0:2a15bd367891 27 }
shimogamo 0:2a15bd367891 28
shimogamo 0:2a15bd367891 29
shimogamo 0:2a15bd367891 30 //非線形にする時用
shimogamo 0:2a15bd367891 31 double ControllerManager::calc(double doublex,int intx){
shimogamo 0:2a15bd367891 32 return doublex;
shimogamo 0:2a15bd367891 33 // return intx+pow(doublex-intx,3);
shimogamo 0:2a15bd367891 34 }
shimogamo 0:2a15bd367891 35
shimogamo 0:2a15bd367891 36
shimogamo 0:2a15bd367891 37
shimogamo 0:2a15bd367891 38
shimogamo 0:2a15bd367891 39
shimogamo 0:2a15bd367891 40
shimogamo 0:2a15bd367891 41
shimogamo 4:650af94bf062 42 /*
shimogamo 0:2a15bd367891 43 void ControllerManager::initialize(){
shimogamo 0:2a15bd367891 44
shimogamo 0:2a15bd367891 45 if(!Global::initializeswitch){
shimogamo 0:2a15bd367891 46 printf("startcontollerinitialize\r\n");
shimogamo 0:2a15bd367891 47 printf("neutralcontrollerdegree?\r\n");
shimogamo 0:2a15bd367891 48 Global::led1=1;
shimogamo 0:2a15bd367891 49 Global::led2=1;
shimogamo 0:2a15bd367891 50 wait(0.2);
shimogamo 0:2a15bd367891 51 Global::led1=0;
shimogamo 0:2a15bd367891 52 Global::led2=0;
shimogamo 0:2a15bd367891 53 wait(0.2);
shimogamo 0:2a15bd367891 54 Global::led1=1;
shimogamo 0:2a15bd367891 55 Global::led2=1;
shimogamo 0:2a15bd367891 56 wait(0.2);
shimogamo 0:2a15bd367891 57 Global::led1=0;
shimogamo 0:2a15bd367891 58 Global::led2=0;
shimogamo 0:2a15bd367891 59 wait(0.2);
shimogamo 0:2a15bd367891 60 Global::led1=1;
shimogamo 0:2a15bd367891 61 Global::led2=1;
shimogamo 0:2a15bd367891 62 wait(0.2);
shimogamo 0:2a15bd367891 63 Global::led1=0;
shimogamo 0:2a15bd367891 64 Global::led2=0;
shimogamo 0:2a15bd367891 65 wait(0.2);
shimogamo 0:2a15bd367891 66 Global::led1=1;
shimogamo 0:2a15bd367891 67 Global::led2=1;
shimogamo 0:2a15bd367891 68 wait(0.2);
shimogamo 0:2a15bd367891 69 Global::led1=0;
shimogamo 0:2a15bd367891 70 Global::led2=0;
shimogamo 0:2a15bd367891 71 wait(0.2);
shimogamo 0:2a15bd367891 72 Global::led1=1;
shimogamo 0:2a15bd367891 73 Global::led2=1;
shimogamo 0:2a15bd367891 74 wait(0.2);
shimogamo 0:2a15bd367891 75 Global::led1=0;
shimogamo 0:2a15bd367891 76 Global::led2=0;
shimogamo 0:2a15bd367891 77 wait(0.2);
shimogamo 0:2a15bd367891 78 Global::led1=1;
shimogamo 0:2a15bd367891 79 Global::led2=1;
shimogamo 0:2a15bd367891 80 wait(0.2);
shimogamo 0:2a15bd367891 81 Global::led1=0;
shimogamo 0:2a15bd367891 82 Global::led2=0;
shimogamo 0:2a15bd367891 83 neutralpitchdegree=controller.getV();
shimogamo 0:2a15bd367891 84 Global::setneutralpitchdegree(neutralpitchdegree);
shimogamo 0:2a15bd367891 85 neutralyawdegree=controller.getH();
shimogamo 0:2a15bd367891 86 Global::setneutralyawdegree(neutralyawdegree);
shimogamo 0:2a15bd367891 87
shimogamo 0:2a15bd367891 88
shimogamo 0:2a15bd367891 89 printf("neutralpitchdegree=%f\tneutralyawdegree=%f\r\n",neutralpitchdegree,neutralyawdegree);
shimogamo 0:2a15bd367891 90 wait(1.0);
shimogamo 0:2a15bd367891 91 printf("maxcontrollerdegree?\r\n");
shimogamo 0:2a15bd367891 92 Global::led1=1;
shimogamo 0:2a15bd367891 93 wait(0.2);
shimogamo 0:2a15bd367891 94 Global::led1=0;
shimogamo 0:2a15bd367891 95 wait(0.2);
shimogamo 0:2a15bd367891 96 Global::led1=1;
shimogamo 0:2a15bd367891 97 wait(0.2);
shimogamo 0:2a15bd367891 98 Global::led1=0;
shimogamo 0:2a15bd367891 99 wait(0.2);
shimogamo 0:2a15bd367891 100 Global::led1=1;
shimogamo 0:2a15bd367891 101 wait(0.2);
shimogamo 0:2a15bd367891 102 Global::led1=0;
shimogamo 0:2a15bd367891 103 wait(0.2);
shimogamo 0:2a15bd367891 104 Global::led1=1;
shimogamo 0:2a15bd367891 105 wait(0.2);
shimogamo 0:2a15bd367891 106 Global::led1=0;
shimogamo 0:2a15bd367891 107 wait(0.2);
shimogamo 0:2a15bd367891 108 Global::led1=1;
shimogamo 0:2a15bd367891 109 wait(0.2);
shimogamo 0:2a15bd367891 110 Global::led1=0;
shimogamo 0:2a15bd367891 111 while(Global::initializeswitch){
shimogamo 0:2a15bd367891 112 Global::led1=1;
shimogamo 0:2a15bd367891 113 wait(0.1);
shimogamo 0:2a15bd367891 114 }
shimogamo 0:2a15bd367891 115 maxpitchdegree=controller.getV();
shimogamo 0:2a15bd367891 116 Global::setmaxpitchdegree(maxpitchdegree);
shimogamo 0:2a15bd367891 117 maxyawdegree=controller.getH();
shimogamo 0:2a15bd367891 118 Global::setmaxyawdegree(maxyawdegree);
shimogamo 0:2a15bd367891 119 printf("maxpitchdegree=%f\tmaxyawdegree=%f\r\n",maxpitchdegree,maxyawdegree);
shimogamo 0:2a15bd367891 120 Global::led1=0;
shimogamo 0:2a15bd367891 121 wait(1.0);
shimogamo 0:2a15bd367891 122 printf("mincontrollerdegree?\r\n");
shimogamo 0:2a15bd367891 123 Global::led2=1;
shimogamo 0:2a15bd367891 124 wait(0.2);
shimogamo 0:2a15bd367891 125 Global::led2=0;
shimogamo 0:2a15bd367891 126 wait(0.2);
shimogamo 0:2a15bd367891 127 Global::led2=1;
shimogamo 0:2a15bd367891 128 wait(0.2);
shimogamo 0:2a15bd367891 129 Global::led2=0;
shimogamo 0:2a15bd367891 130 wait(0.2);
shimogamo 0:2a15bd367891 131 Global::led2=1;
shimogamo 0:2a15bd367891 132 wait(0.2);
shimogamo 0:2a15bd367891 133 Global::led2=0;
shimogamo 0:2a15bd367891 134 wait(0.2);
shimogamo 0:2a15bd367891 135 Global::led2=1;
shimogamo 0:2a15bd367891 136 wait(0.2);
shimogamo 0:2a15bd367891 137 Global::led2=0;
shimogamo 0:2a15bd367891 138 wait(0.2);
shimogamo 0:2a15bd367891 139 Global::led2=1;
shimogamo 0:2a15bd367891 140 wait(0.2);
shimogamo 0:2a15bd367891 141 Global::led2=0;
shimogamo 0:2a15bd367891 142 while(Global::initializeswitch){
shimogamo 0:2a15bd367891 143 Global::led2=1;
shimogamo 0:2a15bd367891 144 wait(0.1);
shimogamo 0:2a15bd367891 145 }
shimogamo 0:2a15bd367891 146 minpitchdegree=controller.getV();
shimogamo 0:2a15bd367891 147 Global::setminpitchdegree(minpitchdegree);
shimogamo 0:2a15bd367891 148 minyawdegree=controller.getH();
shimogamo 0:2a15bd367891 149 Global::setminyawdegree(minyawdegree);
shimogamo 0:2a15bd367891 150 printf("minpitchdegree=%f\tminyawdegree=%f\r\n",minpitchdegree,minyawdegree);
shimogamo 0:2a15bd367891 151 Global::led2=0;
shimogamo 0:2a15bd367891 152 wait(1.0);
shimogamo 0:2a15bd367891 153 }
shimogamo 0:2a15bd367891 154 printf("endcontollerinitialize\r\n");
shimogamo 0:2a15bd367891 155 wait(1.0);
shimogamo 4:650af94bf062 156 }*/
shimogamo 0:2a15bd367891 157
shimogamo 0:2a15bd367891 158 void ControllerManager::update(){
shimogamo 5:9a1ec02229dd 159 maxpitch = Global::getmaxpitch();
shimogamo 5:9a1ec02229dd 160 minpitch = Global::getminpitch();
shimogamo 5:9a1ec02229dd 161 maxyaw = Global::getmaxyaw();
shimogamo 5:9a1ec02229dd 162 minyaw = Global::getminyaw();
shimogamo 5:9a1ec02229dd 163 maxpitchdegree=Global::getmaxpitchdegree();
shimogamo 5:9a1ec02229dd 164 neutralpitchdegree=Global::getneutralpitchdegree();
shimogamo 5:9a1ec02229dd 165 minpitchdegree=Global::getminpitchdegree();
shimogamo 5:9a1ec02229dd 166 maxyawdegree=Global::getmaxyawdegree();
shimogamo 5:9a1ec02229dd 167 neutralyawdegree=Global::getneutralyawdegree();
shimogamo 5:9a1ec02229dd 168 minyawdegree=Global::getminyawdegree();
shimogamo 5:9a1ec02229dd 169
shimogamo 4:650af94bf062 170 Global::setpitch(getpitch());
shimogamo 4:650af94bf062 171 Global::setyaw(getyaw());
shimogamo 3:e3c41153e5fe 172 // printf("%f %f\n", getpitch(), getyaw());
shimogamo 0:2a15bd367891 173 }
shimogamo 0:2a15bd367891 174
shimogamo 0:2a15bd367891 175
shimogamo 0:2a15bd367891 176
shimogamo 0:2a15bd367891 177
shimogamo 0:2a15bd367891 178
shimogamo 0:2a15bd367891 179
shimogamo 0:2a15bd367891 180
shimogamo 0:2a15bd367891 181
shimogamo 0:2a15bd367891 182
shimogamo 2:e0f1e8662b8c 183 //設定した最大、最小値を超えない値を返す
shimogamo 4:650af94bf062 184 static float clamp(double value, double min, double max) {
shimogamo 0:2a15bd367891 185 if(value < min) {
shimogamo 0:2a15bd367891 186 return min;
shimogamo 0:2a15bd367891 187 } else if(value > max) {
shimogamo 0:2a15bd367891 188 return max;
shimogamo 0:2a15bd367891 189 } else {
shimogamo 0:2a15bd367891 190 return value;
shimogamo 0:2a15bd367891 191 }
shimogamo 0:2a15bd367891 192 }
shimogamo 7:6f7bd18ce796 193 //ニュートラル基準でピッチ、ヨーとも[-1,1]となるような値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
shimogamo 0:2a15bd367891 194 double ControllerManager::pitchratio(){
shimogamo 7:6f7bd18ce796 195 if(controller.getV() < neutralpitchdegree){
shimogamo 7:6f7bd18ce796 196 return clamp((controller.getV() - neutralpitchdegree) / (neutralpitchdegree - minpitchdegree), -1, 0);
shimogamo 7:6f7bd18ce796 197 }else{
shimogamo 7:6f7bd18ce796 198 return clamp((controller.getV() - neutralpitchdegree) / (maxpitchdegree - neutralpitchdegree), 0, 1);
shimogamo 7:6f7bd18ce796 199 }
shimogamo 0:2a15bd367891 200 }
shimogamo 0:2a15bd367891 201 double ControllerManager::yawratio(){
shimogamo 7:6f7bd18ce796 202 if(controller.getH() < neutralyawdegree){
shimogamo 7:6f7bd18ce796 203 return clamp((controller.getH() - neutralyawdegree) / (neutralyawdegree - minyawdegree), -1, 0);
shimogamo 7:6f7bd18ce796 204 }else{
shimogamo 7:6f7bd18ce796 205 return clamp((controller.getH() - neutralyawdegree) / (maxyawdegree - neutralyawdegree), 0, 1);
shimogamo 7:6f7bd18ce796 206 }
shimogamo 0:2a15bd367891 207 }
shimogamo 0:2a15bd367891 208
shimogamo 0:2a15bd367891 209
shimogamo 2:e0f1e8662b8c 210 //pitchratioを設定したmaxpitch,minpitch倍にする
shimogamo 0:2a15bd367891 211 double ControllerManager::doublepitch(double pitchratio){
shimogamo 0:2a15bd367891 212 if(pitchratio<0){
shimogamo 0:2a15bd367891 213 return clamp(-pitchratio*minpitch,minpitch,maxpitch);
shimogamo 0:2a15bd367891 214 }else{
shimogamo 0:2a15bd367891 215 return clamp(pitchratio*maxpitch,minpitch,maxpitch);
shimogamo 0:2a15bd367891 216 }
shimogamo 0:2a15bd367891 217 }
shimogamo 0:2a15bd367891 218 double ControllerManager::doubleyaw(double yawratio){
shimogamo 0:2a15bd367891 219 if(yawratio<0){
shimogamo 0:2a15bd367891 220 return clamp(-yawratio*minyaw,minyaw,maxyaw);
shimogamo 0:2a15bd367891 221 }else{
shimogamo 0:2a15bd367891 222 return clamp(yawratio*maxyaw,minyaw,maxyaw);
shimogamo 0:2a15bd367891 223 }
shimogamo 0:2a15bd367891 224 }
shimogamo 0:2a15bd367891 225 int ControllerManager::intpitch(double pitchratio){
shimogamo 0:2a15bd367891 226 if(pitchratio<0){
shimogamo 0:2a15bd367891 227 return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 228 }else{
shimogamo 0:2a15bd367891 229 return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 230 }
shimogamo 0:2a15bd367891 231 }
shimogamo 0:2a15bd367891 232 int ControllerManager::intyaw(double yawratio){
shimogamo 0:2a15bd367891 233 if(yawratio<0){
shimogamo 0:2a15bd367891 234 return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 235 }else{
shimogamo 0:2a15bd367891 236 return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 237 }
shimogamo 0:2a15bd367891 238 }
shimogamo 0:2a15bd367891 239
shimogamo 0:2a15bd367891 240
shimogamo 0:2a15bd367891 241
shimogamo 0:2a15bd367891 242
shimogamo 0:2a15bd367891 243 double ControllerManager::getpitch(){
shimogamo 0:2a15bd367891 244 return calc(doublepitch(pitchratio()),intpitch(pitchratio()));
shimogamo 0:2a15bd367891 245 }
shimogamo 0:2a15bd367891 246 double ControllerManager::getyaw(){
shimogamo 0:2a15bd367891 247 return calc(doubleyaw(yawratio()),intyaw(yawratio()));
shimogamo 0:2a15bd367891 248 }
shimogamo 0:2a15bd367891 249 double ControllerManager::getpitch(double _pitchratio){
shimogamo 0:2a15bd367891 250 return calc(doublepitch(_pitchratio),intpitch(_pitchratio));
shimogamo 0:2a15bd367891 251 }
shimogamo 0:2a15bd367891 252 double ControllerManager::getyaw(double _yawratio){
shimogamo 0:2a15bd367891 253 return calc(doubleyaw(_yawratio),intyaw(_yawratio));
shimogamo 0:2a15bd367891 254 }