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@5:9a1ec02229dd, 2015-10-09 (annotated)

Committer:
shimogamo
Date:
Fri Oct 09 14:29:32 2015 +0000
Revision:
5:9a1ec02229dd
Parent:
4:650af94bf062
Child:
6:0d9fa7152934
main.cpp??initialize??????????????string????

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