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

Committer:
shimogamo
Date:
Sat Oct 10 20:16:31 2015 +0000
Revision:
6:0d9fa7152934
Parent:
5:9a1ec02229dd
Child:
7:6f7bd18ce796
??????????????csv????????????????????????????????????????????

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