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@4:650af94bf062, 2015-09-29 (annotated)

Committer:
shimogamo
Date:
Tue Sep 29 18:23:29 2015 +0000
Revision:
4:650af94bf062
Parent:
3:e3c41153e5fe
Child:
5:9a1ec02229dd
angle?????Global??????????????????????

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 0:2a15bd367891 12 ControllerManager::ControllerManager(PinName b, PinName h, PinName v)
shimogamo 0:2a15bd367891 13 : controller(b, h, v){
shimogamo 0:2a15bd367891 14 maxpitch = Global::getmaxpitch();//
shimogamo 0:2a15bd367891 15 minpitch = Global::getminpitch();//
shimogamo 0:2a15bd367891 16 maxyaw = Global::getmaxyaw();//
shimogamo 0:2a15bd367891 17 minyaw = Global::getminyaw();//
shimogamo 0:2a15bd367891 18
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 4:650af94bf062 159 Global::setpitch(getpitch());
shimogamo 4:650af94bf062 160 Global::setyaw(getyaw());
shimogamo 3:e3c41153e5fe 161 // printf("%f %f\n", getpitch(), getyaw());
shimogamo 0:2a15bd367891 162
shimogamo 0:2a15bd367891 163 }
shimogamo 0:2a15bd367891 164
shimogamo 0:2a15bd367891 165
shimogamo 0:2a15bd367891 166
shimogamo 0:2a15bd367891 167
shimogamo 0:2a15bd367891 168
shimogamo 0:2a15bd367891 169
shimogamo 0:2a15bd367891 170
shimogamo 0:2a15bd367891 171
shimogamo 0:2a15bd367891 172
shimogamo 2:e0f1e8662b8c 173 //設定した最大、最小値を超えない値を返す
shimogamo 4:650af94bf062 174 static float clamp(double value, double min, double max) {
shimogamo 0:2a15bd367891 175 if(value < min) {
shimogamo 0:2a15bd367891 176 return min;
shimogamo 0:2a15bd367891 177 } else if(value > max) {
shimogamo 0:2a15bd367891 178 return max;
shimogamo 0:2a15bd367891 179 } else {
shimogamo 0:2a15bd367891 180 return value;
shimogamo 0:2a15bd367891 181 }
shimogamo 0:2a15bd367891 182 }
shimogamo 2:e0f1e8662b8c 183 //ニュートラル基準でピッチ、ヨーともに[-1.0,1.0]の範囲で値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
shimogamo 0:2a15bd367891 184 double ControllerManager::pitchratio(){
shimogamo 0:2a15bd367891 185 return 2.0*(controller.getV() - neutralpitchdegree)/(maxpitchdegree-minpitchdegree);
shimogamo 0:2a15bd367891 186 }
shimogamo 0:2a15bd367891 187 double ControllerManager::yawratio(){
shimogamo 0:2a15bd367891 188 return -2.0*(controller.getH() - neutralyawdegree)/(maxpitchdegree-minpitchdegree);
shimogamo 0:2a15bd367891 189 }
shimogamo 0:2a15bd367891 190
shimogamo 0:2a15bd367891 191
shimogamo 0:2a15bd367891 192
shimogamo 0:2a15bd367891 193
shimogamo 0:2a15bd367891 194
shimogamo 2:e0f1e8662b8c 195 //pitchratioを設定したmaxpitch,minpitch倍にする
shimogamo 0:2a15bd367891 196 double ControllerManager::doublepitch(double pitchratio){
shimogamo 0:2a15bd367891 197 if(pitchratio<0){
shimogamo 0:2a15bd367891 198 return clamp(-pitchratio*minpitch,minpitch,maxpitch);
shimogamo 0:2a15bd367891 199 }else{
shimogamo 0:2a15bd367891 200 return clamp(pitchratio*maxpitch,minpitch,maxpitch);
shimogamo 0:2a15bd367891 201 }
shimogamo 0:2a15bd367891 202 }
shimogamo 0:2a15bd367891 203 double ControllerManager::doubleyaw(double yawratio){
shimogamo 0:2a15bd367891 204 if(yawratio<0){
shimogamo 0:2a15bd367891 205 return clamp(-yawratio*minyaw,minyaw,maxyaw);
shimogamo 0:2a15bd367891 206 }else{
shimogamo 0:2a15bd367891 207 return clamp(yawratio*maxyaw,minyaw,maxyaw);
shimogamo 0:2a15bd367891 208 }
shimogamo 0:2a15bd367891 209 }
shimogamo 0:2a15bd367891 210 int ControllerManager::intpitch(double pitchratio){
shimogamo 0:2a15bd367891 211 if(pitchratio<0){
shimogamo 0:2a15bd367891 212 return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 213 }else{
shimogamo 0:2a15bd367891 214 return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 215 }
shimogamo 0:2a15bd367891 216 }
shimogamo 0:2a15bd367891 217 int ControllerManager::intyaw(double yawratio){
shimogamo 0:2a15bd367891 218 if(yawratio<0){
shimogamo 0:2a15bd367891 219 return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 220 }else{
shimogamo 0:2a15bd367891 221 return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 222 }
shimogamo 0:2a15bd367891 223 }
shimogamo 0:2a15bd367891 224
shimogamo 0:2a15bd367891 225
shimogamo 0:2a15bd367891 226
shimogamo 0:2a15bd367891 227
shimogamo 0:2a15bd367891 228 double ControllerManager::getpitch(){
shimogamo 0:2a15bd367891 229 return calc(doublepitch(pitchratio()),intpitch(pitchratio()));
shimogamo 0:2a15bd367891 230 }
shimogamo 0:2a15bd367891 231 double ControllerManager::getyaw(){
shimogamo 0:2a15bd367891 232 return calc(doubleyaw(yawratio()),intyaw(yawratio()));
shimogamo 0:2a15bd367891 233 }
shimogamo 0:2a15bd367891 234 double ControllerManager::getpitch(double _pitchratio){
shimogamo 0:2a15bd367891 235 return calc(doublepitch(_pitchratio),intpitch(_pitchratio));
shimogamo 0:2a15bd367891 236 }
shimogamo 0:2a15bd367891 237 double ControllerManager::getyaw(double _yawratio){
shimogamo 0:2a15bd367891 238 return calc(doubleyaw(_yawratio),intyaw(_yawratio));
shimogamo 0:2a15bd367891 239 }