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@2:e0f1e8662b8c, 2015-09-28 (annotated)

Committer:
shimogamo
Date:
Mon Sep 28 10:33:07 2015 +0000
Revision:
2:e0f1e8662b8c
Parent:
0:2a15bd367891
Child:
3:e3c41153e5fe
Standard

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 0:2a15bd367891 7
shimogamo 0:2a15bd367891 8 //係数変更でパイロットに合わせるたぶん使わん
shimogamo 0:2a15bd367891 9 ControllerManager::ControllerManager(PinName b, PinName h, PinName v)
shimogamo 0:2a15bd367891 10 : controller(b, h, v){
shimogamo 0:2a15bd367891 11 maxpitch = Global::getmaxpitch();//
shimogamo 0:2a15bd367891 12 minpitch = Global::getminpitch();//
shimogamo 0:2a15bd367891 13 maxyaw = Global::getmaxyaw();//
shimogamo 0:2a15bd367891 14 minyaw = Global::getminyaw();//
shimogamo 0:2a15bd367891 15
shimogamo 0:2a15bd367891 16
shimogamo 0:2a15bd367891 17 maxpitchdegree=1.0;
shimogamo 0:2a15bd367891 18 neutralpitchdegree=0.5;
shimogamo 0:2a15bd367891 19 minpitchdegree=0.0;
shimogamo 0:2a15bd367891 20 maxyawdegree=1.0;
shimogamo 0:2a15bd367891 21 neutralyawdegree=0.5;
shimogamo 0:2a15bd367891 22 minyawdegree=0.0;
shimogamo 0:2a15bd367891 23
shimogamo 0:2a15bd367891 24 }
shimogamo 0:2a15bd367891 25
shimogamo 0:2a15bd367891 26
shimogamo 0:2a15bd367891 27 //非線形にする時用
shimogamo 0:2a15bd367891 28 double ControllerManager::calc(double doublex,int intx){
shimogamo 0:2a15bd367891 29 return doublex;
shimogamo 0:2a15bd367891 30 // return intx+pow(doublex-intx,3);
shimogamo 0:2a15bd367891 31 }
shimogamo 0:2a15bd367891 32
shimogamo 0:2a15bd367891 33
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 void ControllerManager::initialize(){
shimogamo 0:2a15bd367891 41
shimogamo 0:2a15bd367891 42 if(!Global::initializeswitch){
shimogamo 0:2a15bd367891 43 printf("startcontollerinitialize\r\n");
shimogamo 0:2a15bd367891 44 printf("neutralcontrollerdegree?\r\n");
shimogamo 0:2a15bd367891 45 Global::led1=1;
shimogamo 0:2a15bd367891 46 Global::led2=1;
shimogamo 0:2a15bd367891 47 wait(0.2);
shimogamo 0:2a15bd367891 48 Global::led1=0;
shimogamo 0:2a15bd367891 49 Global::led2=0;
shimogamo 0:2a15bd367891 50 wait(0.2);
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 neutralpitchdegree=controller.getV();
shimogamo 0:2a15bd367891 81 Global::setneutralpitchdegree(neutralpitchdegree);
shimogamo 0:2a15bd367891 82 neutralyawdegree=controller.getH();
shimogamo 0:2a15bd367891 83 Global::setneutralyawdegree(neutralyawdegree);
shimogamo 0:2a15bd367891 84
shimogamo 0:2a15bd367891 85
shimogamo 0:2a15bd367891 86 printf("neutralpitchdegree=%f\tneutralyawdegree=%f\r\n",neutralpitchdegree,neutralyawdegree);
shimogamo 0:2a15bd367891 87 wait(1.0);
shimogamo 0:2a15bd367891 88 printf("maxcontrollerdegree?\r\n");
shimogamo 0:2a15bd367891 89 Global::led1=1;
shimogamo 0:2a15bd367891 90 wait(0.2);
shimogamo 0:2a15bd367891 91 Global::led1=0;
shimogamo 0:2a15bd367891 92 wait(0.2);
shimogamo 0:2a15bd367891 93 Global::led1=1;
shimogamo 0:2a15bd367891 94 wait(0.2);
shimogamo 0:2a15bd367891 95 Global::led1=0;
shimogamo 0:2a15bd367891 96 wait(0.2);
shimogamo 0:2a15bd367891 97 Global::led1=1;
shimogamo 0:2a15bd367891 98 wait(0.2);
shimogamo 0:2a15bd367891 99 Global::led1=0;
shimogamo 0:2a15bd367891 100 wait(0.2);
shimogamo 0:2a15bd367891 101 Global::led1=1;
shimogamo 0:2a15bd367891 102 wait(0.2);
shimogamo 0:2a15bd367891 103 Global::led1=0;
shimogamo 0:2a15bd367891 104 wait(0.2);
shimogamo 0:2a15bd367891 105 Global::led1=1;
shimogamo 0:2a15bd367891 106 wait(0.2);
shimogamo 0:2a15bd367891 107 Global::led1=0;
shimogamo 0:2a15bd367891 108 while(Global::initializeswitch){
shimogamo 0:2a15bd367891 109 Global::led1=1;
shimogamo 0:2a15bd367891 110 wait(0.1);
shimogamo 0:2a15bd367891 111 }
shimogamo 0:2a15bd367891 112 maxpitchdegree=controller.getV();
shimogamo 0:2a15bd367891 113 Global::setmaxpitchdegree(maxpitchdegree);
shimogamo 0:2a15bd367891 114 maxyawdegree=controller.getH();
shimogamo 0:2a15bd367891 115 Global::setmaxyawdegree(maxyawdegree);
shimogamo 0:2a15bd367891 116 printf("maxpitchdegree=%f\tmaxyawdegree=%f\r\n",maxpitchdegree,maxyawdegree);
shimogamo 0:2a15bd367891 117 Global::led1=0;
shimogamo 0:2a15bd367891 118 wait(1.0);
shimogamo 0:2a15bd367891 119 printf("mincontrollerdegree?\r\n");
shimogamo 0:2a15bd367891 120 Global::led2=1;
shimogamo 0:2a15bd367891 121 wait(0.2);
shimogamo 0:2a15bd367891 122 Global::led2=0;
shimogamo 0:2a15bd367891 123 wait(0.2);
shimogamo 0:2a15bd367891 124 Global::led2=1;
shimogamo 0:2a15bd367891 125 wait(0.2);
shimogamo 0:2a15bd367891 126 Global::led2=0;
shimogamo 0:2a15bd367891 127 wait(0.2);
shimogamo 0:2a15bd367891 128 Global::led2=1;
shimogamo 0:2a15bd367891 129 wait(0.2);
shimogamo 0:2a15bd367891 130 Global::led2=0;
shimogamo 0:2a15bd367891 131 wait(0.2);
shimogamo 0:2a15bd367891 132 Global::led2=1;
shimogamo 0:2a15bd367891 133 wait(0.2);
shimogamo 0:2a15bd367891 134 Global::led2=0;
shimogamo 0:2a15bd367891 135 wait(0.2);
shimogamo 0:2a15bd367891 136 Global::led2=1;
shimogamo 0:2a15bd367891 137 wait(0.2);
shimogamo 0:2a15bd367891 138 Global::led2=0;
shimogamo 0:2a15bd367891 139 while(Global::initializeswitch){
shimogamo 0:2a15bd367891 140 Global::led2=1;
shimogamo 0:2a15bd367891 141 wait(0.1);
shimogamo 0:2a15bd367891 142 }
shimogamo 0:2a15bd367891 143 minpitchdegree=controller.getV();
shimogamo 0:2a15bd367891 144 Global::setminpitchdegree(minpitchdegree);
shimogamo 0:2a15bd367891 145 minyawdegree=controller.getH();
shimogamo 0:2a15bd367891 146 Global::setminyawdegree(minyawdegree);
shimogamo 0:2a15bd367891 147 printf("minpitchdegree=%f\tminyawdegree=%f\r\n",minpitchdegree,minyawdegree);
shimogamo 0:2a15bd367891 148 Global::led2=0;
shimogamo 0:2a15bd367891 149 wait(1.0);
shimogamo 0:2a15bd367891 150 }
shimogamo 0:2a15bd367891 151 printf("endcontollerinitialize\r\n");
shimogamo 0:2a15bd367891 152 wait(1.0);
shimogamo 0:2a15bd367891 153 }
shimogamo 0:2a15bd367891 154
shimogamo 0:2a15bd367891 155 void ControllerManager::update(){
shimogamo 0:2a15bd367891 156 Global::setpitch(getpitch() + Global::gettrimpitch());
shimogamo 0:2a15bd367891 157 Global::setyaw(getyaw() + Global::gettrimyaw());
shimogamo 0:2a15bd367891 158 printf("cont\n");
shimogamo 0:2a15bd367891 159
shimogamo 0:2a15bd367891 160 }
shimogamo 0:2a15bd367891 161
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 2:e0f1e8662b8c 170 //設定した最大、最小値を超えない値を返す
shimogamo 0:2a15bd367891 171 static float clamp(float value, float min, float max) {
shimogamo 0:2a15bd367891 172 if(value < min) {
shimogamo 0:2a15bd367891 173 return min;
shimogamo 0:2a15bd367891 174 } else if(value > max) {
shimogamo 0:2a15bd367891 175 return max;
shimogamo 0:2a15bd367891 176 } else {
shimogamo 0:2a15bd367891 177 return value;
shimogamo 0:2a15bd367891 178 }
shimogamo 0:2a15bd367891 179 }
shimogamo 2:e0f1e8662b8c 180 //ニュートラル基準でピッチ、ヨーともに[-1.0,1.0]の範囲で値を返す(maxpitchdegree=1.0;neutralpitchdegree=0.5;minpitchdegree=0.0;のとき)
shimogamo 0:2a15bd367891 181 double ControllerManager::pitchratio(){
shimogamo 0:2a15bd367891 182 return 2.0*(controller.getV() - neutralpitchdegree)/(maxpitchdegree-minpitchdegree);
shimogamo 0:2a15bd367891 183 }
shimogamo 0:2a15bd367891 184 double ControllerManager::yawratio(){
shimogamo 0:2a15bd367891 185 return -2.0*(controller.getH() - neutralyawdegree)/(maxpitchdegree-minpitchdegree);
shimogamo 0:2a15bd367891 186 }
shimogamo 0:2a15bd367891 187
shimogamo 0:2a15bd367891 188
shimogamo 0:2a15bd367891 189
shimogamo 0:2a15bd367891 190
shimogamo 0:2a15bd367891 191
shimogamo 2:e0f1e8662b8c 192 //pitchratioを設定したmaxpitch,minpitch倍にする
shimogamo 0:2a15bd367891 193 double ControllerManager::doublepitch(double pitchratio){
shimogamo 0:2a15bd367891 194 if(pitchratio<0){
shimogamo 0:2a15bd367891 195 return clamp(-pitchratio*minpitch,minpitch,maxpitch);
shimogamo 0:2a15bd367891 196 }else{
shimogamo 0:2a15bd367891 197 return clamp(pitchratio*maxpitch,minpitch,maxpitch);
shimogamo 0:2a15bd367891 198 }
shimogamo 0:2a15bd367891 199 }
shimogamo 0:2a15bd367891 200 double ControllerManager::doubleyaw(double yawratio){
shimogamo 0:2a15bd367891 201 if(yawratio<0){
shimogamo 0:2a15bd367891 202 return clamp(-yawratio*minyaw,minyaw,maxyaw);
shimogamo 0:2a15bd367891 203 }else{
shimogamo 0:2a15bd367891 204 return clamp(yawratio*maxyaw,minyaw,maxyaw);
shimogamo 0:2a15bd367891 205 }
shimogamo 0:2a15bd367891 206 }
shimogamo 0:2a15bd367891 207 int ControllerManager::intpitch(double pitchratio){
shimogamo 0:2a15bd367891 208 if(pitchratio<0){
shimogamo 0:2a15bd367891 209 return clamp(((int)(pitchratio*(-2*minpitch+1))-1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 210 }else{
shimogamo 0:2a15bd367891 211 return clamp(((int)(pitchratio*(2*maxpitch+1))+1)/2,minpitch,maxpitch);
shimogamo 0:2a15bd367891 212 }
shimogamo 0:2a15bd367891 213 }
shimogamo 0:2a15bd367891 214 int ControllerManager::intyaw(double yawratio){
shimogamo 0:2a15bd367891 215 if(yawratio<0){
shimogamo 0:2a15bd367891 216 return clamp(((int)(yawratio*(-2*minyaw+1))-1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 217 }else{
shimogamo 0:2a15bd367891 218 return clamp(((int)(yawratio*(2*maxyaw+1))+1)/2,minyaw,maxyaw);
shimogamo 0:2a15bd367891 219 }
shimogamo 0:2a15bd367891 220 }
shimogamo 0:2a15bd367891 221
shimogamo 0:2a15bd367891 222
shimogamo 0:2a15bd367891 223
shimogamo 0:2a15bd367891 224
shimogamo 0:2a15bd367891 225 double ControllerManager::getpitch(){
shimogamo 0:2a15bd367891 226 return calc(doublepitch(pitchratio()),intpitch(pitchratio()));
shimogamo 0:2a15bd367891 227 }
shimogamo 0:2a15bd367891 228 double ControllerManager::getyaw(){
shimogamo 0:2a15bd367891 229 return calc(doubleyaw(yawratio()),intyaw(yawratio()));
shimogamo 0:2a15bd367891 230 }
shimogamo 0:2a15bd367891 231 double ControllerManager::getpitch(double _pitchratio){
shimogamo 0:2a15bd367891 232 return calc(doublepitch(_pitchratio),intpitch(_pitchratio));
shimogamo 0:2a15bd367891 233 }
shimogamo 0:2a15bd367891 234 double ControllerManager::getyaw(double _yawratio){
shimogamo 0:2a15bd367891 235 return calc(doubleyaw(_yawratio),intyaw(_yawratio));
shimogamo 0:2a15bd367891 236 }