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

main.cpp@8:ca92cb674004, 2015-10-13 (annotated)

Committer:
shimogamo
Date:
Tue Oct 13 13:10:21 2015 +0000
Revision:
8:ca92cb674004
Parent:
7:6f7bd18ce796
Child:
9:d1fc0805ec7d
play????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimogamo 8:ca92cb674004 1 //@todo trimのパラメータをpcから設定，csvファイルに追加
shimogamo 8:ca92cb674004 2 //@todo initをクラス化
shimogamo 8:ca92cb674004 3
shimogamo 8:ca92cb674004 4 //@todo androidとの通信
shimogamo 8:ca92cb674004 5 //@todo xbeeとの通信
shimogamo 8:ca92cb674004 6
shimogamo 8:ca92cb674004 7 //@todo 高度計の追加
shimogamo 8:ca92cb674004 8
shimogamo 0:2a15bd367891 9 #include "mbed.h"
shimogamo 0:2a15bd367891 10 #include "rtos.h"
shimogamo 0:2a15bd367891 11 #include "Global.h"
shimogamo 0:2a15bd367891 12 #include "ServoManager.h"
shimogamo 0:2a15bd367891 13 #include "ControllerManager.h"
shimogamo 0:2a15bd367891 14 #include "Trim.h"
shimogamo 0:2a15bd367891 15 #include "Cadence.h"
shimogamo 0:2a15bd367891 16 #include "Airspeed.h"
shimogamo 0:2a15bd367891 17 #include "Altitude.h"
shimogamo 0:2a15bd367891 18 #include "Display.h"
shimogamo 0:2a15bd367891 19 #include "XBee.h"
shimogamo 5:9a1ec02229dd 20 #include <string>
shimogamo 6:0d9fa7152934 21 #include <iostream>
shimogamo 5:9a1ec02229dd 22
shimogamo 5:9a1ec02229dd 23 #define BUFMAX 20
shimogamo 5:9a1ec02229dd 24
shimogamo 5:9a1ec02229dd 25 RawSerial pc(USBTX, USBRX);
shimogamo 5:9a1ec02229dd 26 Queue<char, BUFMAX> queue;
shimogamo 0:2a15bd367891 27
shimogamo 0:2a15bd367891 28
shimogamo 0:2a15bd367891 29 Display display(p9, p10);
shimogamo 0:2a15bd367891 30 XBee xbee(p13, p14);
shimogamo 8:ca92cb674004 31 Trim trim(p16, p17);
shimogamo 0:2a15bd367891 32 ControllerManager controllerManager(p18,p19,p20);
shimogamo 0:2a15bd367891 33 ServoManager servoManager(p21, p22);
shimogamo 0:2a15bd367891 34 Airspeed airspeed(p26, NC, NC);
shimogamo 0:2a15bd367891 35 Cadence cadence(p29, p30, NC);
shimogamo 0:2a15bd367891 36
shimogamo 0:2a15bd367891 37
shimogamo 5:9a1ec02229dd 38 void pc_rx(){
shimogamo 5:9a1ec02229dd 39 while(pc.readable()==1){
shimogamo 5:9a1ec02229dd 40 char buf = pc.getc();
shimogamo 5:9a1ec02229dd 41 queue.put((char*)buf);
shimogamo 0:2a15bd367891 42 }
shimogamo 0:2a15bd367891 43 }
shimogamo 0:2a15bd367891 44
shimogamo 5:9a1ec02229dd 45
shimogamo 5:9a1ec02229dd 46 void initializeTask(void const *pvParametersd){
shimogamo 5:9a1ec02229dd 47 std::string strbuf;
shimogamo 6:0d9fa7152934 48
shimogamo 5:9a1ec02229dd 49 while(1){
shimogamo 5:9a1ec02229dd 50 osEvent evt = queue.get();
shimogamo 5:9a1ec02229dd 51 if(evt.status == osEventMessage){
shimogamo 5:9a1ec02229dd 52 char temp = evt.value.v;
shimogamo 5:9a1ec02229dd 53 strbuf.push_back(temp);
shimogamo 6:0d9fa7152934 54 if(temp == '\n'){
shimogamo 6:0d9fa7152934 55 std::string::size_type spaceIndex = strbuf.find(" ");
shimogamo 5:9a1ec02229dd 56
shimogamo 7:6f7bd18ce796 57 if (spaceIndex != std::string::npos) {//数値データがあるコマンド（" "がある場合）
shimogamo 5:9a1ec02229dd 58 char *gomi;
shimogamo 5:9a1ec02229dd 59 std::string command = strbuf.substr(0, spaceIndex);
shimogamo 7:6f7bd18ce796 60 double num = strtod(strbuf.substr(spaceIndex+1, strbuf.size()-2).c_str(), &gomi);//strbuf.size()-1には'\n'が入っている
shimogamo 5:9a1ec02229dd 61
shimogamo 5:9a1ec02229dd 62 pc.printf("coms=%s, num=%f\n", command.c_str(), num);
shimogamo 5:9a1ec02229dd 63
shimogamo 5:9a1ec02229dd 64 if(command == "eleneu"){
shimogamo 5:9a1ec02229dd 65 Global::setneutralpitch(num);
shimogamo 5:9a1ec02229dd 66 }else if(command == "elemax"){
shimogamo 5:9a1ec02229dd 67 Global::setmaxpitch(num);
shimogamo 5:9a1ec02229dd 68 }else if(command == "elemin"){
shimogamo 5:9a1ec02229dd 69 Global::setminpitch(num);
shimogamo 5:9a1ec02229dd 70 }else if(command == "rudneu"){
shimogamo 5:9a1ec02229dd 71 Global::setneutralyaw(num);
shimogamo 5:9a1ec02229dd 72 }else if(command == "rudmax"){
shimogamo 5:9a1ec02229dd 73 Global::setmaxyaw(num);
shimogamo 5:9a1ec02229dd 74 }else if(command == "rudmin"){
shimogamo 5:9a1ec02229dd 75 Global::setminyaw(num);
shimogamo 7:6f7bd18ce796 76
shimogamo 7:6f7bd18ce796 77 }else if(command == "eleneudeg"){
shimogamo 7:6f7bd18ce796 78 Global::setneutralpitchdegree(num);
shimogamo 7:6f7bd18ce796 79 }else if(command == "elemaxdeg"){
shimogamo 7:6f7bd18ce796 80 Global::setmaxpitchdegree(num);
shimogamo 7:6f7bd18ce796 81 }else if(command == "elemindeg"){
shimogamo 7:6f7bd18ce796 82 Global::setminpitchdegree(num);
shimogamo 7:6f7bd18ce796 83 }else if(command == "rudneudeg"){
shimogamo 7:6f7bd18ce796 84 Global::setneutralyawdegree(num);
shimogamo 7:6f7bd18ce796 85 }else if(command == "rudmaxdeg"){
shimogamo 7:6f7bd18ce796 86 Global::setmaxyawdegree(num);
shimogamo 7:6f7bd18ce796 87 }else if(command == "rudmindeg"){
shimogamo 7:6f7bd18ce796 88 Global::setminyawdegree(num);
shimogamo 8:ca92cb674004 89
shimogamo 8:ca92cb674004 90 }else if(command == "elemaxplay"){
shimogamo 8:ca92cb674004 91 Global::setmaxpitchplayratio(num);
shimogamo 8:ca92cb674004 92 }else if(command == "eleminplay"){
shimogamo 8:ca92cb674004 93 Global::setminpitchplayratio(num);
shimogamo 8:ca92cb674004 94 }else if(command == "rudmaxplay"){
shimogamo 8:ca92cb674004 95 Global::setmaxyawplayratio(num);
shimogamo 8:ca92cb674004 96 }else if(command == "rudminplay"){
shimogamo 8:ca92cb674004 97 Global::setminyawplayratio(num);
shimogamo 7:6f7bd18ce796 98
shimogamo 7:6f7bd18ce796 99 }else{
shimogamo 7:6f7bd18ce796 100 pc.printf("Invalid Input\n");
shimogamo 5:9a1ec02229dd 101 }
shimogamo 5:9a1ec02229dd 102
shimogamo 7:6f7bd18ce796 103 }else{//数値データがないコマンド（" "がない場合）
shimogamo 8:ca92cb674004 104
shimogamo 7:6f7bd18ce796 105 std::string command = strbuf.substr(0,strbuf.size()-2);
shimogamo 8:ca92cb674004 106 if(command == "eleneu"){
shimogamo 8:ca92cb674004 107 Global::setneutralpitch(Global::getpitch());
shimogamo 8:ca92cb674004 108 }else if(command == "elemax"){
shimogamo 8:ca92cb674004 109 Global::setmaxpitch(Global::getpitch());
shimogamo 8:ca92cb674004 110 }else if(command == "elemin"){
shimogamo 8:ca92cb674004 111 Global::setminpitch(Global::getpitch());
shimogamo 8:ca92cb674004 112 }else if(command == "rudneu"){
shimogamo 8:ca92cb674004 113 Global::setneutralyaw(Global::getyaw());
shimogamo 8:ca92cb674004 114 }else if(command == "rudmax"){
shimogamo 8:ca92cb674004 115 Global::setmaxyaw(Global::getyaw());
shimogamo 8:ca92cb674004 116 }else if(command == "rudmin"){
shimogamo 8:ca92cb674004 117 Global::setminyaw(Global::getyaw());
shimogamo 8:ca92cb674004 118
shimogamo 8:ca92cb674004 119 }else if(command == "eleneudeg"){
shimogamo 8:ca92cb674004 120 Global::setneutralpitchdegree(Global::getpitchdegree());
shimogamo 8:ca92cb674004 121 }else if(command == "elemaxdeg"){
shimogamo 8:ca92cb674004 122 Global::setmaxpitchdegree(Global::getpitchdegree());
shimogamo 8:ca92cb674004 123 }else if(command == "elemindeg"){
shimogamo 8:ca92cb674004 124 Global::setminpitchdegree(Global::getpitchdegree());
shimogamo 8:ca92cb674004 125 }else if(command == "rudneudeg"){
shimogamo 8:ca92cb674004 126 Global::setneutralyawdegree(Global::getyawdegree());
shimogamo 8:ca92cb674004 127 }else if(command == "rudmaxdeg"){
shimogamo 8:ca92cb674004 128 Global::setmaxyawdegree(Global::getyawdegree());
shimogamo 8:ca92cb674004 129 }else if(command == "rudmindeg"){
shimogamo 8:ca92cb674004 130 Global::setminyawdegree(Global::getyawdegree());
shimogamo 8:ca92cb674004 131
shimogamo 8:ca92cb674004 132 }else if(command == "show"){
shimogamo 8:ca92cb674004 133 pc.printf("------Servo Parameter------\n");
shimogamo 7:6f7bd18ce796 134 pc.printf("elevatorNeutral = %f\n", Global::getneutralpitch());
shimogamo 7:6f7bd18ce796 135 pc.printf("elevatorMax = %f\n", Global::getmaxpitch());
shimogamo 7:6f7bd18ce796 136 pc.printf("elevatorMin = %f\n", Global::getminpitch());
shimogamo 7:6f7bd18ce796 137 pc.printf("rudderNeutral = %f\n", Global::getneutralyaw());
shimogamo 7:6f7bd18ce796 138 pc.printf("rudderMax = %f\n", Global::getmaxyaw());
shimogamo 7:6f7bd18ce796 139 pc.printf("rudderMin = %f\n", Global::getminyaw());
shimogamo 8:ca92cb674004 140 pc.printf("----Controller Parameter----\n");
shimogamo 8:ca92cb674004 141 pc.printf("elevatorNeutral = %f\n", Global::getneutralpitchdegree());
shimogamo 8:ca92cb674004 142 pc.printf("elevatorMax = %f\n", Global::getmaxpitchdegree());
shimogamo 8:ca92cb674004 143 pc.printf("elevatorMin = %f\n", Global::getminpitchdegree());
shimogamo 8:ca92cb674004 144 pc.printf("rudderNeutral = %f\n", Global::getneutralyawdegree());
shimogamo 8:ca92cb674004 145 pc.printf("rudderMax = %f\n", Global::getmaxyawdegree());
shimogamo 8:ca92cb674004 146 pc.printf("rudderMin = %f\n", Global::getminyawdegree());
shimogamo 8:ca92cb674004 147 pc.printf("------Controller Play------\n");
shimogamo 8:ca92cb674004 148 pc.printf("elevatorMaxPlay = %f\n", Global::getmaxpitchplayratio());
shimogamo 8:ca92cb674004 149 pc.printf("elevatorMinPlay = %f\n", Global::getminpitchplayratio());
shimogamo 8:ca92cb674004 150 pc.printf("rudderMaxPlay = %f\n", Global::getmaxyawplayratio());
shimogamo 8:ca92cb674004 151 pc.printf("rudderMinPlay = %f\n", Global::getminyawplayratio());
shimogamo 8:ca92cb674004 152
shimogamo 7:6f7bd18ce796 153 }else{
shimogamo 7:6f7bd18ce796 154 pc.printf("Invalid Input\n");
shimogamo 7:6f7bd18ce796 155 }
shimogamo 8:ca92cb674004 156 //ここで，ニュートラル情報をLocalFileに保存
shimogamo 8:ca92cb674004 157 Global::filewrite();
shimogamo 5:9a1ec02229dd 158 }
shimogamo 7:6f7bd18ce796 159 strbuf.clear();
shimogamo 5:9a1ec02229dd 160 }
shimogamo 5:9a1ec02229dd 161 Thread::wait(10);
shimogamo 5:9a1ec02229dd 162 if(strbuf.size() >= BUFMAX){
shimogamo 5:9a1ec02229dd 163 pc.printf("\nInput Error ... ");
shimogamo 5:9a1ec02229dd 164 strbuf.clear();
shimogamo 5:9a1ec02229dd 165 pc.printf("Buffer clear\n");
shimogamo 5:9a1ec02229dd 166 }
shimogamo 5:9a1ec02229dd 167 }
shimogamo 5:9a1ec02229dd 168 }
shimogamo 5:9a1ec02229dd 169 }
shimogamo 5:9a1ec02229dd 170
shimogamo 5:9a1ec02229dd 171
shimogamo 0:2a15bd367891 172 void controlTask(void const *pvParameters){
shimogamo 0:2a15bd367891 173 while(1){
shimogamo 2:e0f1e8662b8c 174 trim.update();
shimogamo 0:2a15bd367891 175 controllerManager.update();
shimogamo 0:2a15bd367891 176 servoManager.update();
shimogamo 0:2a15bd367891 177 Thread::wait(50);
shimogamo 0:2a15bd367891 178 }
shimogamo 0:2a15bd367891 179 }
shimogamo 0:2a15bd367891 180
shimogamo 0:2a15bd367891 181 void airspeedTask(void const *pvParameters){
shimogamo 0:2a15bd367891 182 while(1){
shimogamo 0:2a15bd367891 183 airspeed.update();
shimogamo 0:2a15bd367891 184 Thread::wait(200);
shimogamo 0:2a15bd367891 185 }
shimogamo 0:2a15bd367891 186 }
shimogamo 0:2a15bd367891 187
shimogamo 0:2a15bd367891 188 void cadenceTask(void const *pvParameters){
shimogamo 0:2a15bd367891 189 while(1){
shimogamo 0:2a15bd367891 190 cadence.update();
shimogamo 0:2a15bd367891 191 Thread::wait(200);
shimogamo 0:2a15bd367891 192 }
shimogamo 0:2a15bd367891 193 }
shimogamo 0:2a15bd367891 194
shimogamo 0:2a15bd367891 195 void displayTask(void const *pvParameters){
shimogamo 0:2a15bd367891 196 while(1){
shimogamo 0:2a15bd367891 197 display.update();
shimogamo 0:2a15bd367891 198 Thread::wait(50);
shimogamo 0:2a15bd367891 199 }
shimogamo 0:2a15bd367891 200 }
shimogamo 0:2a15bd367891 201
shimogamo 0:2a15bd367891 202 void xbeeTask(void const *pvParameters){
shimogamo 0:2a15bd367891 203 while(1){
shimogamo 0:2a15bd367891 204 xbee.update();
shimogamo 0:2a15bd367891 205 Thread::wait(50);
shimogamo 0:2a15bd367891 206 }
shimogamo 0:2a15bd367891 207 }
shimogamo 0:2a15bd367891 208
shimogamo 0:2a15bd367891 209
shimogamo 0:2a15bd367891 210 int main(void){
shimogamo 0:2a15bd367891 211 printf("start\n");
shimogamo 5:9a1ec02229dd 212
shimogamo 5:9a1ec02229dd 213 pc.attach(pc_rx,Serial::RxIrq);
shimogamo 5:9a1ec02229dd 214
shimogamo 6:0d9fa7152934 215 Global::initialize();
shimogamo 5:9a1ec02229dd 216 Thread InitializeTask(initializeTask);
shimogamo 0:2a15bd367891 217 Thread ControlTask(controlTask);
shimogamo 0:2a15bd367891 218 Thread AirspeedTask(airspeedTask);
shimogamo 0:2a15bd367891 219 Thread CadenceTask(cadenceTask);
shimogamo 0:2a15bd367891 220 Thread DisplayTask(displayTask);
shimogamo 0:2a15bd367891 221 Thread XbeeTask(xbeeTask);
shimogamo 0:2a15bd367891 222 printf("Task end\n");
shimogamo 0:2a15bd367891 223
shimogamo 0:2a15bd367891 224 Thread::wait(osWaitForever);
shimogamo 0:2a15bd367891 225 }