ryou sato / Mbed 2 deprecated NUCLEO-F401RE_S11059-02DT_SG92R

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

main.cpp@1:e5b0268067e4, 2021-02-25 (annotated)

Committer:
ryousato
Date:
Thu Feb 25 09:14:24 2021 +0000
Revision:
1:e5b0268067e4
Parent:
0:5637e966d2f5
Child:
2:a10de3b5e0f6
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryousato 0:5637e966d2f5 1 //メインプログラム・UARTコマンド受信
ryousato 0:5637e966d2f5 2 #include "mbed.h"
ryousato 0:5637e966d2f5 3 #include <cstdio>
ryousato 0:5637e966d2f5 4 #include <string>
ryousato 0:5637e966d2f5 5 #include "ColorSensor.h"
ryousato 0:5637e966d2f5 6
ryousato 0:5637e966d2f5 7 #define SirialComSpeed 38400 //シリアル通信のBPS設定
ryousato 0:5637e966d2f5 8
ryousato 0:5637e966d2f5 9 #define CommandNumber (sizeof CommandList) / (sizeof(CdListTable)) //コマンドの数（CommandNumber）
ryousato 0:5637e966d2f5 10
ryousato 0:5637e966d2f5 11 //変数定義
ryousato 0:5637e966d2f5 12 int servoflag = 0; //サーボホーン位置情報格納用
ryousato 0:5637e966d2f5 13 int *pservoflag; //ポインター
ryousato 0:5637e966d2f5 14 int off_width = 1720; //サーボホーン初期位置パルス幅[us]
ryousato 0:5637e966d2f5 15 int *poff_width; //ポインター
ryousato 0:5637e966d2f5 16 int on_width = 1500; //サーボホーン終端位置パルス幅[us]
ryousato 0:5637e966d2f5 17 int *pon_width; //ポインター
ryousato 0:5637e966d2f5 18
ryousato 0:5637e966d2f5 19 //シリアル関連
ryousato 0:5637e966d2f5 20 RawSerial SerialCom(SERIAL_TX, SERIAL_RX); //シリアル通信
ryousato 0:5637e966d2f5 21 std::string RcvCharStr = ""; //シリアル通信受信文字格納用
ryousato 0:5637e966d2f5 22
ryousato 0:5637e966d2f5 23 //ピン設定
ryousato 0:5637e966d2f5 24 DigitalOut led(LED1);
ryousato 0:5637e966d2f5 25 PwmOut servopwm(PA_0);
ryousato 0:5637e966d2f5 26 InterruptIn det0(PA_4); //POWER PKG 設置検知
ryousato 0:5637e966d2f5 27 InterruptIn det1(PC_1); //MAIN PKG 設置検知
ryousato 0:5637e966d2f5 28 InterruptIn det2(PC_0); //レバー降ろし検知
ryousato 0:5637e966d2f5 29 DigitalOut cmuxen(PA_8); //カラーセンサー用マルチプレクサENピン
ryousato 0:5637e966d2f5 30 BusOut cmuxaddr(PC_7,PA_9); //カラーセンサー用マルチプレクサADDRバス
ryousato 0:5637e966d2f5 31 DigitalOut rmuxen(PA_10); //リレー用マルチプレクサENピン
ryousato 0:5637e966d2f5 32 BusOut rmuxaddr(PB_10,PB_4,PB_5,PB_3); //リレー用マルチプレクサADDRバス
ryousato 0:5637e966d2f5 33 DigitalInOut relayctl1(PA_11, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 34 DigitalInOut relayctl2(PB_12, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 35 DigitalInOut relayctl3(PB_2, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 36 DigitalInOut relayctl4(PB_1, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 37 DigitalInOut relayctl5(PB_15, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 38 DigitalInOut relayctl6(PB_14, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 39 DigitalInOut relayctl7(PB_13, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 40 DigitalInOut relayctl8(PC_4, PIN_OUTPUT, OpenDrain, 0); //個別リレー用制御
ryousato 0:5637e966d2f5 41
ryousato 0:5637e966d2f5 42 static std::string none;
ryousato 0:5637e966d2f5 43
ryousato 0:5637e966d2f5 44 //関数宣言
ryousato 0:5637e966d2f5 45 int help_mode(std::string para);
ryousato 0:5637e966d2f5 46 int relay_set(std::string para); //リレー用制御
ryousato 1:e5b0268067e4 47 int initial(std::string para); //初期化
ryousato 0:5637e966d2f5 48 int led_lum_check(std::string para); //カラーセンサー用マルチプレクサ制御 ＋ 輝度確認
ryousato 0:5637e966d2f5 49 int led_all_check(std::string para);
ryousato 0:5637e966d2f5 50 int servo_turn(std::string para);
ryousato 0:5637e966d2f5 51 int pkg_detection(std::string para);
ryousato 0:5637e966d2f5 52 void main_uart_receive(); //コマンド文字列受信処理
ryousato 0:5637e966d2f5 53 void risedet_interrupt();
ryousato 0:5637e966d2f5 54 void falldet_interrupt();
ryousato 0:5637e966d2f5 55
ryousato 0:5637e966d2f5 56 //クラス名の定義
ryousato 0:5637e966d2f5 57 CsMode CsMode;
ryousato 0:5637e966d2f5 58
ryousato 0:5637e966d2f5 59 //コマンドの宣言
ryousato 0:5637e966d2f5 60 typedef struct {
ryousato 0:5637e966d2f5 61 std::string Commond;
ryousato 0:5637e966d2f5 62 int (* CommandFunction)(std::string para);
ryousato 0:5637e966d2f5 63 } CdListTable;
ryousato 0:5637e966d2f5 64
ryousato 0:5637e966d2f5 65 //コマンド一覧（左側がコマンド、右側が飛び先の関数）
ryousato 0:5637e966d2f5 66 CdListTable CommandList[] = {
ryousato 0:5637e966d2f5 67 {"help", help_mode},
ryousato 0:5637e966d2f5 68 {"led", led_lum_check},
ryousato 0:5637e966d2f5 69 {"led all", led_all_check},
ryousato 0:5637e966d2f5 70 {"relay", relay_set},
ryousato 0:5637e966d2f5 71 {"servo", servo_turn},
ryousato 0:5637e966d2f5 72 {"det", pkg_detection},
ryousato 1:e5b0268067e4 73 {"initial", initial},
ryousato 0:5637e966d2f5 74 };
ryousato 0:5637e966d2f5 75
ryousato 0:5637e966d2f5 76 //ヘルプ用コマンド一覧
ryousato 0:5637e966d2f5 77 std::string HelpStr[] = {
ryousato 0:5637e966d2f5 78 "* Commands List **********************************************************",
ryousato 0:5637e966d2f5 79 "relayALLoff : relay control",
ryousato 0:5637e966d2f5 80 "relay1,2{on|off}",
ryousato 0:5637e966d2f5 81 "relay3{on|off}",
ryousato 0:5637e966d2f5 82 "relay4{on|off}",
ryousato 0:5637e966d2f5 83 "relay5{on|off}",
ryousato 0:5637e966d2f5 84 "relay21{on|off}",
ryousato 0:5637e966d2f5 85 "relay22,23{on|off}",
ryousato 0:5637e966d2f5 86 "relay24,25{on|off}",
ryousato 0:5637e966d2f5 87 "relay26{on|off}",
ryousato 0:5637e966d2f5 88 "relay26on20ms",
ryousato 0:5637e966d2f5 89 "relay26on73ms",
ryousato 0:5637e966d2f5 90 "relayMUXoff",
ryousato 0:5637e966d2f5 91 "relay6on",
ryousato 0:5637e966d2f5 92 "relay7on",
ryousato 0:5637e966d2f5 93 "relay8on",
ryousato 0:5637e966d2f5 94 "relay9on",
ryousato 0:5637e966d2f5 95 "relay10on",
ryousato 0:5637e966d2f5 96 "relay11on",
ryousato 0:5637e966d2f5 97 "relay12on",
ryousato 0:5637e966d2f5 98 "relay13on",
ryousato 0:5637e966d2f5 99 "relay14on",
ryousato 0:5637e966d2f5 100 "relay15on",
ryousato 0:5637e966d2f5 101 "relay16on",
ryousato 0:5637e966d2f5 102 "relay17on",
ryousato 0:5637e966d2f5 103 "relay18on",
ryousato 0:5637e966d2f5 104 "relay19on",
ryousato 0:5637e966d2f5 105 "relay20on",
ryousato 0:5637e966d2f5 106 "servo{on|off} : servo control",
ryousato 0:5637e966d2f5 107 "led{1|2|3} : LED luminance check",
ryousato 0:5637e966d2f5 108 "det : pkg,lever detection",
ryousato 0:5637e966d2f5 109 "**************************************************************************",
ryousato 0:5637e966d2f5 110 "", //helpの終わりを示す
ryousato 0:5637e966d2f5 111 };
ryousato 0:5637e966d2f5 112
ryousato 0:5637e966d2f5 113 //ヘルプ表示
ryousato 0:5637e966d2f5 114 int help_mode(std::string para)
ryousato 0:5637e966d2f5 115 {
ryousato 0:5637e966d2f5 116 int tmpCount = 0;
ryousato 0:5637e966d2f5 117
ryousato 0:5637e966d2f5 118 //HelpStrを空文字が出てくるまで繰り返し表示
ryousato 0:5637e966d2f5 119 while(HelpStr[tmpCount] != "") {
ryousato 0:5637e966d2f5 120 SerialCom.printf("%s\r\n",HelpStr[tmpCount].c_str());
ryousato 0:5637e966d2f5 121 tmpCount++;
ryousato 0:5637e966d2f5 122 }
ryousato 0:5637e966d2f5 123 return 0;
ryousato 0:5637e966d2f5 124 }
ryousato 0:5637e966d2f5 125
ryousato 0:5637e966d2f5 126 //リレー制御
ryousato 0:5637e966d2f5 127 int relay_set(std::string para)
ryousato 0:5637e966d2f5 128 {
ryousato 0:5637e966d2f5 129 //基板とレバーが未検知の時はコマンドを実行しない
ryousato 0:5637e966d2f5 130 int intdet0 = det0;
ryousato 0:5637e966d2f5 131 int intdet1 = det1;
ryousato 0:5637e966d2f5 132 int intdet2 = det2;
ryousato 0:5637e966d2f5 133 if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 134 SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
ryousato 0:5637e966d2f5 135 return 0;
ryousato 0:5637e966d2f5 136 }
ryousato 0:5637e966d2f5 137
ryousato 0:5637e966d2f5 138 //パラメータが
ryousato 0:5637e966d2f5 139 if(para.substr(0)=="ALLoff") {
ryousato 0:5637e966d2f5 140 relayctl1 = 0;
ryousato 0:5637e966d2f5 141 relayctl2 = 0;
ryousato 0:5637e966d2f5 142 relayctl3 = 0;
ryousato 0:5637e966d2f5 143 relayctl4 = 0;
ryousato 0:5637e966d2f5 144 relayctl5 = 0;
ryousato 0:5637e966d2f5 145 relayctl6 = 0;
ryousato 0:5637e966d2f5 146 relayctl7 = 0;
ryousato 0:5637e966d2f5 147 relayctl8 = 0;
ryousato 0:5637e966d2f5 148 rmuxen = 1;
ryousato 0:5637e966d2f5 149 rmuxaddr.write(0);
ryousato 0:5637e966d2f5 150 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 151 return 0;
ryousato 0:5637e966d2f5 152 } else if (para.substr(0)=="1,2on") {
ryousato 0:5637e966d2f5 153 relayctl1 = 1;
ryousato 0:5637e966d2f5 154
ryousato 0:5637e966d2f5 155 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 156 return 0;
ryousato 0:5637e966d2f5 157 } else if (para.substr(0)=="1,2off") {
ryousato 0:5637e966d2f5 158 relayctl1 = 0;
ryousato 0:5637e966d2f5 159 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 160 return 0;
ryousato 0:5637e966d2f5 161 } else if (para.substr(0)=="3on") {
ryousato 0:5637e966d2f5 162 relayctl2 = 1;
ryousato 0:5637e966d2f5 163 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 164 return 0;
ryousato 0:5637e966d2f5 165 } else if (para.substr(0)=="3off") {
ryousato 0:5637e966d2f5 166 relayctl2 = 0;
ryousato 0:5637e966d2f5 167 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 168 return 0;
ryousato 0:5637e966d2f5 169 } else if (para.substr(0)=="4on") {
ryousato 0:5637e966d2f5 170 relayctl3 = 1;
ryousato 0:5637e966d2f5 171 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 172 return 0;
ryousato 0:5637e966d2f5 173 } else if (para.substr(0)=="4off") {
ryousato 0:5637e966d2f5 174 relayctl3 = 0;
ryousato 0:5637e966d2f5 175 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 176 return 0;
ryousato 0:5637e966d2f5 177 } else if (para.substr(0)=="5on") {
ryousato 0:5637e966d2f5 178 relayctl4 = 1;
ryousato 0:5637e966d2f5 179 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 180 return 0;
ryousato 0:5637e966d2f5 181 } else if (para.substr(0)=="5off") {
ryousato 0:5637e966d2f5 182 relayctl4 = 0;
ryousato 0:5637e966d2f5 183 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 184 return 0;
ryousato 0:5637e966d2f5 185 } else if (para.substr(0)=="21on") {
ryousato 0:5637e966d2f5 186 relayctl5 = 1;
ryousato 0:5637e966d2f5 187 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 188 return 0;
ryousato 0:5637e966d2f5 189 } else if (para.substr(0)=="21off") {
ryousato 0:5637e966d2f5 190 relayctl5 = 0;
ryousato 0:5637e966d2f5 191 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 192 return 0;
ryousato 0:5637e966d2f5 193 } else if (para.substr(0)=="22,23on") {
ryousato 0:5637e966d2f5 194 relayctl6 = 1;
ryousato 0:5637e966d2f5 195 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 196 return 0;
ryousato 0:5637e966d2f5 197 } else if (para.substr(0)=="22,23off") {
ryousato 0:5637e966d2f5 198 relayctl6 = 0;
ryousato 0:5637e966d2f5 199 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 200 return 0;
ryousato 0:5637e966d2f5 201 } else if (para.substr(0)=="24,25on") {
ryousato 0:5637e966d2f5 202 relayctl7 = 1;
ryousato 0:5637e966d2f5 203 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 204 return 0;
ryousato 0:5637e966d2f5 205 } else if (para.substr(0)=="24,25off") {
ryousato 0:5637e966d2f5 206 relayctl7 = 0;
ryousato 0:5637e966d2f5 207 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 208 return 0;
ryousato 0:5637e966d2f5 209 } else if (para.substr(0)=="26on") {
ryousato 0:5637e966d2f5 210 relayctl8 = 1;
ryousato 0:5637e966d2f5 211 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 212 return 0;
ryousato 0:5637e966d2f5 213 } else if (para.substr(0)=="26off") {
ryousato 0:5637e966d2f5 214 relayctl8 = 0;
ryousato 0:5637e966d2f5 215 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 216 return 0;
ryousato 0:5637e966d2f5 217 } else if (para.substr(0)=="26on20ms") {
ryousato 0:5637e966d2f5 218 relayctl8 = 1;
ryousato 0:5637e966d2f5 219 wait(0.02);
ryousato 0:5637e966d2f5 220 relayctl8 = 0;
ryousato 0:5637e966d2f5 221 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 222 return 0;
ryousato 0:5637e966d2f5 223 } else if (para.substr(0)=="26on73ms") {
ryousato 0:5637e966d2f5 224 relayctl8 = 1;
ryousato 0:5637e966d2f5 225 wait(0.073);
ryousato 0:5637e966d2f5 226 relayctl8 = 0;
ryousato 0:5637e966d2f5 227 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 228 return 0;
ryousato 0:5637e966d2f5 229 } else if (para.substr(0)=="MUXoff") {
ryousato 0:5637e966d2f5 230 rmuxen = 1;
ryousato 0:5637e966d2f5 231 wait(0.01);
ryousato 0:5637e966d2f5 232 rmuxaddr.write(0);
ryousato 0:5637e966d2f5 233 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 234 return 0;
ryousato 0:5637e966d2f5 235 } else if (para.substr(0)=="6on") {
ryousato 0:5637e966d2f5 236 rmuxen = 1;
ryousato 0:5637e966d2f5 237 wait(0.01);
ryousato 0:5637e966d2f5 238 rmuxaddr.write(1);
ryousato 0:5637e966d2f5 239 wait(0.01);
ryousato 0:5637e966d2f5 240 rmuxen = 0;
ryousato 0:5637e966d2f5 241 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 242 return 0;
ryousato 0:5637e966d2f5 243 } else if (para.substr(0)=="7on") {
ryousato 0:5637e966d2f5 244 rmuxen = 1;
ryousato 0:5637e966d2f5 245 wait(0.01);
ryousato 0:5637e966d2f5 246 rmuxaddr.write(2);
ryousato 0:5637e966d2f5 247 wait(0.01);
ryousato 0:5637e966d2f5 248 rmuxen = 0;
ryousato 0:5637e966d2f5 249 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 250 return 0;
ryousato 0:5637e966d2f5 251 } else if (para.substr(0)=="8on") {
ryousato 0:5637e966d2f5 252 rmuxen = 1;
ryousato 0:5637e966d2f5 253 wait(0.01);
ryousato 0:5637e966d2f5 254 rmuxaddr.write(3);
ryousato 0:5637e966d2f5 255 wait(0.01);
ryousato 0:5637e966d2f5 256 rmuxen = 0;
ryousato 0:5637e966d2f5 257 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 258 return 0;
ryousato 0:5637e966d2f5 259 } else if (para.substr(0)=="9on") {
ryousato 0:5637e966d2f5 260 rmuxen = 1;
ryousato 0:5637e966d2f5 261 wait(0.01);
ryousato 0:5637e966d2f5 262 rmuxaddr.write(4);
ryousato 0:5637e966d2f5 263 wait(0.01);
ryousato 0:5637e966d2f5 264 rmuxen = 0;
ryousato 0:5637e966d2f5 265 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 266 return 0;
ryousato 0:5637e966d2f5 267 } else if (para.substr(0)=="10on") {
ryousato 0:5637e966d2f5 268 rmuxen = 1;
ryousato 0:5637e966d2f5 269 wait(0.01);
ryousato 0:5637e966d2f5 270 rmuxaddr.write(5);
ryousato 0:5637e966d2f5 271 wait(0.01);
ryousato 0:5637e966d2f5 272 rmuxen = 0;
ryousato 0:5637e966d2f5 273 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 274 return 0;
ryousato 0:5637e966d2f5 275 } else if (para.substr(0)=="11on") {
ryousato 0:5637e966d2f5 276 rmuxen = 1;
ryousato 0:5637e966d2f5 277 wait(0.01);
ryousato 0:5637e966d2f5 278 rmuxaddr.write(6);
ryousato 0:5637e966d2f5 279 wait(0.01);
ryousato 0:5637e966d2f5 280 rmuxen = 0;
ryousato 0:5637e966d2f5 281 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 282 return 0;
ryousato 0:5637e966d2f5 283 } else if (para.substr(0)=="12on") {
ryousato 0:5637e966d2f5 284 rmuxen = 1;
ryousato 0:5637e966d2f5 285 wait(0.01);
ryousato 0:5637e966d2f5 286 rmuxaddr.write(7);
ryousato 0:5637e966d2f5 287 wait(0.01);
ryousato 0:5637e966d2f5 288 rmuxen = 0;
ryousato 0:5637e966d2f5 289 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 290 return 0;
ryousato 0:5637e966d2f5 291 } else if (para.substr(0)=="13on") {
ryousato 0:5637e966d2f5 292 rmuxen = 1;
ryousato 0:5637e966d2f5 293 wait(0.01);
ryousato 0:5637e966d2f5 294 rmuxaddr.write(8);
ryousato 0:5637e966d2f5 295 wait(0.01);
ryousato 0:5637e966d2f5 296 rmuxen = 0;
ryousato 0:5637e966d2f5 297 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 298 return 0;
ryousato 0:5637e966d2f5 299 } else if (para.substr(0)=="14on") {
ryousato 0:5637e966d2f5 300 rmuxen = 1;
ryousato 0:5637e966d2f5 301 wait(0.01);
ryousato 0:5637e966d2f5 302 rmuxaddr.write(9);
ryousato 0:5637e966d2f5 303 wait(0.01);
ryousato 0:5637e966d2f5 304 rmuxen = 0;
ryousato 0:5637e966d2f5 305 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 306 return 0;
ryousato 0:5637e966d2f5 307 } else if (para.substr(0)=="15on") {
ryousato 0:5637e966d2f5 308 rmuxen = 1;
ryousato 0:5637e966d2f5 309 wait(0.01);
ryousato 0:5637e966d2f5 310 rmuxaddr.write(10);
ryousato 0:5637e966d2f5 311 wait(0.01);
ryousato 0:5637e966d2f5 312 rmuxen = 0;
ryousato 0:5637e966d2f5 313 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 314 return 0;
ryousato 0:5637e966d2f5 315 } else if (para.substr(0)=="16on") {
ryousato 0:5637e966d2f5 316 rmuxen = 1;
ryousato 0:5637e966d2f5 317 wait(0.01);
ryousato 0:5637e966d2f5 318 rmuxaddr.write(11);
ryousato 0:5637e966d2f5 319 wait(0.01);
ryousato 0:5637e966d2f5 320 rmuxen = 0;
ryousato 0:5637e966d2f5 321 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 322 return 0;
ryousato 0:5637e966d2f5 323 } else if (para.substr(0)=="17on") {
ryousato 0:5637e966d2f5 324 rmuxen = 1;
ryousato 0:5637e966d2f5 325 wait(0.01);
ryousato 0:5637e966d2f5 326 rmuxaddr.write(12);
ryousato 0:5637e966d2f5 327 wait(0.01);
ryousato 0:5637e966d2f5 328 rmuxen = 0;
ryousato 0:5637e966d2f5 329 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 330 return 0;
ryousato 0:5637e966d2f5 331 } else if (para.substr(0)=="18on") {
ryousato 0:5637e966d2f5 332 rmuxen = 1;
ryousato 0:5637e966d2f5 333 wait(0.01);
ryousato 0:5637e966d2f5 334 rmuxaddr.write(13);
ryousato 0:5637e966d2f5 335 wait(0.01);
ryousato 0:5637e966d2f5 336 rmuxen = 0;
ryousato 0:5637e966d2f5 337 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 338 return 0;
ryousato 0:5637e966d2f5 339 } else if (para.substr(0)=="19on") {
ryousato 0:5637e966d2f5 340 rmuxen = 1;
ryousato 0:5637e966d2f5 341 wait(0.01);
ryousato 0:5637e966d2f5 342 rmuxaddr.write(14);
ryousato 0:5637e966d2f5 343 wait(0.01);
ryousato 0:5637e966d2f5 344 rmuxen = 0;
ryousato 0:5637e966d2f5 345 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 346 return 0;
ryousato 0:5637e966d2f5 347 } else if (para.substr(0)=="20on") {
ryousato 0:5637e966d2f5 348 rmuxen = 1;
ryousato 0:5637e966d2f5 349 wait(0.01);
ryousato 0:5637e966d2f5 350 rmuxaddr.write(15);
ryousato 0:5637e966d2f5 351 wait(0.01);
ryousato 0:5637e966d2f5 352 rmuxen = 0;
ryousato 0:5637e966d2f5 353 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 354 return 0;
ryousato 0:5637e966d2f5 355 //パラメータが上記以外の場合
ryousato 0:5637e966d2f5 356 } else {
ryousato 0:5637e966d2f5 357 SerialCom.printf(" ERROR!\r\n");
ryousato 0:5637e966d2f5 358 return 0;
ryousato 0:5637e966d2f5 359 }
ryousato 0:5637e966d2f5 360 //return 0;
ryousato 0:5637e966d2f5 361 }
ryousato 0:5637e966d2f5 362
ryousato 0:5637e966d2f5 363 //LEDの明るさ取得
ryousato 0:5637e966d2f5 364 int led_lum_check(std::string para)
ryousato 0:5637e966d2f5 365 {
ryousato 0:5637e966d2f5 366 //基板とレバーが未検知の時はコマンドを実行しない
ryousato 0:5637e966d2f5 367 int intdet0 = det0;
ryousato 0:5637e966d2f5 368 int intdet1 = det1;
ryousato 0:5637e966d2f5 369 int intdet2 = det2;
ryousato 0:5637e966d2f5 370 if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 371 SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
ryousato 0:5637e966d2f5 372 return 0;
ryousato 0:5637e966d2f5 373 }
ryousato 0:5637e966d2f5 374
ryousato 0:5637e966d2f5 375 int tmpInt = atoi(para.c_str()); //パラメータを数字に変換
ryousato 0:5637e966d2f5 376 cmuxen = 1; //マルチプレクサのENピンをHIGHにする
ryousato 0:5637e966d2f5 377 wait(0.01);
ryousato 0:5637e966d2f5 378
ryousato 0:5637e966d2f5 379 //ADDRバス切替
ryousato 0:5637e966d2f5 380 cmuxaddr.write(tmpInt);
ryousato 0:5637e966d2f5 381 wait(0.01);
ryousato 0:5637e966d2f5 382
ryousato 0:5637e966d2f5 383 cmuxen = 0; //マルチプレクサのENピンをLOWにする
ryousato 0:5637e966d2f5 384 wait(0.01);
ryousato 0:5637e966d2f5 385
ryousato 0:5637e966d2f5 386 //選択したLED表示
ryousato 0:5637e966d2f5 387 //SerialCom.printf ("LED%d_", tmpInt);
ryousato 0:5637e966d2f5 388
ryousato 0:5637e966d2f5 389 //LEDの輝度表示
ryousato 0:5637e966d2f5 390 CsMode.SensorMain();
ryousato 0:5637e966d2f5 391
ryousato 0:5637e966d2f5 392 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 393 return 0;
ryousato 0:5637e966d2f5 394 }
ryousato 0:5637e966d2f5 395
ryousato 0:5637e966d2f5 396 int pkg_detection(std::string para)
ryousato 0:5637e966d2f5 397 {
ryousato 0:5637e966d2f5 398 int ppkgdet = det0;
ryousato 0:5637e966d2f5 399 int mpkgdet = det1;
ryousato 0:5637e966d2f5 400 int leverdet = det2;
ryousato 0:5637e966d2f5 401
ryousato 0:5637e966d2f5 402 SerialCom.printf("%d%d%d\r\n", leverdet, mpkgdet, ppkgdet);
ryousato 0:5637e966d2f5 403 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 404 return 0;
ryousato 0:5637e966d2f5 405 }
ryousato 0:5637e966d2f5 406
ryousato 0:5637e966d2f5 407 //コマンド文字列解析
ryousato 0:5637e966d2f5 408 void ReceiveCmdAnalyze()
ryousato 0:5637e966d2f5 409 {
ryousato 0:5637e966d2f5 410 //入力された文字列を1文字ずつ解析（文字列の長さ分まで繰り返す）
ryousato 0:5637e966d2f5 411 for(int y=0; y<=RcvCharStr.size(); y++) {
ryousato 0:5637e966d2f5 412
ryousato 0:5637e966d2f5 413 //コマンド全部と比較し一致しているのがあれば処理開始(前方一致)
ryousato 0:5637e966d2f5 414 for (int i=0; i<CommandNumber; i++) {
ryousato 0:5637e966d2f5 415 if(RcvCharStr.substr(0,y) == CommandList[i].Commond) {
ryousato 0:5637e966d2f5 416
ryousato 0:5637e966d2f5 417 //コマンド以降の文字列を引数にして関数呼び出し
ryousato 0:5637e966d2f5 418 (* *CommandList[i].CommandFunction)(RcvCharStr.substr(y));
ryousato 0:5637e966d2f5 419
ryousato 0:5637e966d2f5 420 //文字列のバッファクリア
ryousato 0:5637e966d2f5 421 RcvCharStr = "";
ryousato 0:5637e966d2f5 422 return;
ryousato 0:5637e966d2f5 423 }
ryousato 0:5637e966d2f5 424 }
ryousato 0:5637e966d2f5 425 }
ryousato 0:5637e966d2f5 426 //エラーメッセージ表示
ryousato 0:5637e966d2f5 427 SerialCom.printf (" ERROR!\r\n\r\n");
ryousato 0:5637e966d2f5 428 RcvCharStr = "";
ryousato 0:5637e966d2f5 429 return;
ryousato 0:5637e966d2f5 430 }
ryousato 0:5637e966d2f5 431
ryousato 0:5637e966d2f5 432 //UART受信
ryousato 0:5637e966d2f5 433 void main_uart_receive()
ryousato 0:5637e966d2f5 434 {
ryousato 0:5637e966d2f5 435 std::string tmpStr = ""; //シリアル通信受信文字格納(1文字)
ryousato 0:5637e966d2f5 436 tmpStr = SerialCom.getc(); //受信文字を配列に追加
ryousato 0:5637e966d2f5 437 SerialCom.printf("%s",tmpStr.c_str()); //エコーバック出力
ryousato 0:5637e966d2f5 438
ryousato 0:5637e966d2f5 439 //①改行文字が来た場合コマンド解析へ
ryousato 0:5637e966d2f5 440 if(tmpStr == "\r") {
ryousato 0:5637e966d2f5 441 SerialCom.printf ("\r\n"); //TeraTerm改行
ryousato 0:5637e966d2f5 442 ReceiveCmdAnalyze(); //コマンド解析
ryousato 0:5637e966d2f5 443 return;
ryousato 0:5637e966d2f5 444
ryousato 0:5637e966d2f5 445 //②バックスペースが来た場合は文字列の末尾を削除
ryousato 0:5637e966d2f5 446 } else if(tmpStr == "\b") {
ryousato 0:5637e966d2f5 447
ryousato 0:5637e966d2f5 448 //1文字もない場合は何もしない
ryousato 0:5637e966d2f5 449 if(RcvCharStr.size()>0) {
ryousato 0:5637e966d2f5 450 RcvCharStr.erase(RcvCharStr.length()-1,1);
ryousato 0:5637e966d2f5 451 }
ryousato 0:5637e966d2f5 452
ryousato 0:5637e966d2f5 453 //③上記以外の場合は受信した文字をRcvCharStrへ格納
ryousato 0:5637e966d2f5 454 } else {
ryousato 0:5637e966d2f5 455 RcvCharStr += tmpStr;
ryousato 0:5637e966d2f5 456 }
ryousato 0:5637e966d2f5 457 }
ryousato 0:5637e966d2f5 458
ryousato 0:5637e966d2f5 459 //サーボモータ制御
ryousato 0:5637e966d2f5 460 int servo_turn(std::string para)
ryousato 0:5637e966d2f5 461 {
ryousato 0:5637e966d2f5 462 //オフ時のパルス幅は上記で定義
ryousato 0:5637e966d2f5 463 //off_width = 1550;
ryousato 0:5637e966d2f5 464 //on_width = 1350;
ryousato 0:5637e966d2f5 465
ryousato 0:5637e966d2f5 466 //基板とレバーが未検知の時はコマンドを実行しない
ryousato 0:5637e966d2f5 467 int intdet0 = det0;
ryousato 0:5637e966d2f5 468 int intdet1 = det1;
ryousato 0:5637e966d2f5 469 int intdet2 = det2;
ryousato 0:5637e966d2f5 470 if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 471 SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
ryousato 0:5637e966d2f5 472 return 0;
ryousato 0:5637e966d2f5 473 }
ryousato 0:5637e966d2f5 474
ryousato 0:5637e966d2f5 475 int waitms = 5; //パルス時間変更後の待ち時間(ms)　(動作速度：0.1秒/60度(サーボモータSG92仕様))
ryousato 0:5637e966d2f5 476
ryousato 0:5637e966d2f5 477 //パラメータがオフ(off)の時
ryousato 0:5637e966d2f5 478 if(para.substr(0)=="off") {
ryousato 0:5637e966d2f5 479 if(servoflag == 0) {
ryousato 0:5637e966d2f5 480 SerialCom.printf(" ERROR! already OFF\r\n");
ryousato 0:5637e966d2f5 481 return 0;
ryousato 0:5637e966d2f5 482 } else {
ryousato 0:5637e966d2f5 483 /*
ryousato 0:5637e966d2f5 484 int pulseus = on_width;
ryousato 0:5637e966d2f5 485 servopwm.period_ms(20); //PWMサイクル周期(サーボモータSG92仕様)
ryousato 0:5637e966d2f5 486 while (pulseus <= off_width) {
ryousato 0:5637e966d2f5 487 servopwm.pulsewidth_us(pulseus);
ryousato 0:5637e966d2f5 488 wait_ms(waitms);
ryousato 0:5637e966d2f5 489 pulseus = pulseus + 1;
ryousato 0:5637e966d2f5 490 }
ryousato 0:5637e966d2f5 491 */
ryousato 0:5637e966d2f5 492 int pulseus = off_width;
ryousato 0:5637e966d2f5 493 servopwm.period_ms(20);
ryousato 0:5637e966d2f5 494 servopwm.pulsewidth_us(pulseus);
ryousato 0:5637e966d2f5 495 servoflag = 0; //サーボホーン位置情報
ryousato 0:5637e966d2f5 496 }
ryousato 0:5637e966d2f5 497 }
ryousato 0:5637e966d2f5 498
ryousato 0:5637e966d2f5 499 //パラメータがオン(on)の時
ryousato 0:5637e966d2f5 500 else if(para.substr(0)=="on") {
ryousato 0:5637e966d2f5 501 //往路
ryousato 0:5637e966d2f5 502 if(servoflag == 1) {
ryousato 0:5637e966d2f5 503 SerialCom.printf(" ERROR! already ON\r\n");
ryousato 0:5637e966d2f5 504 return 0;
ryousato 0:5637e966d2f5 505 } else {
ryousato 0:5637e966d2f5 506 int pulseus = off_width;
ryousato 0:5637e966d2f5 507 servopwm.period_ms(20); //PWMサイクル周期(サーボモータSG92仕様)
ryousato 0:5637e966d2f5 508 while (pulseus >= on_width) {
ryousato 0:5637e966d2f5 509 servopwm.pulsewidth_us(pulseus);
ryousato 0:5637e966d2f5 510 wait_ms(waitms);
ryousato 0:5637e966d2f5 511 pulseus = pulseus - 1;
ryousato 0:5637e966d2f5 512 }
ryousato 0:5637e966d2f5 513 servoflag = 1; //サーボホーン位置情報
ryousato 0:5637e966d2f5 514 }
ryousato 0:5637e966d2f5 515
ryousato 0:5637e966d2f5 516 wait(1); //スイッチを押し切る時間を確保
ryousato 0:5637e966d2f5 517
ryousato 0:5637e966d2f5 518 //復路
ryousato 0:5637e966d2f5 519 if(servoflag == 0) {
ryousato 0:5637e966d2f5 520 SerialCom.printf(" ERROR! already OFF\r\n");
ryousato 0:5637e966d2f5 521 return 0;
ryousato 0:5637e966d2f5 522 } else {
ryousato 0:5637e966d2f5 523 int pulseus = on_width;
ryousato 0:5637e966d2f5 524 servopwm.period_ms(20); //PWMサイクル周期(サーボモータSG92仕様)
ryousato 0:5637e966d2f5 525 while (pulseus <= off_width) {
ryousato 0:5637e966d2f5 526 servopwm.pulsewidth_us(pulseus);
ryousato 0:5637e966d2f5 527 wait_ms(waitms);
ryousato 0:5637e966d2f5 528 pulseus = pulseus + 1;
ryousato 0:5637e966d2f5 529 }
ryousato 0:5637e966d2f5 530 servoflag = 0; //サーボホーン位置情報
ryousato 0:5637e966d2f5 531
ryousato 0:5637e966d2f5 532 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 533 }
ryousato 0:5637e966d2f5 534 }
ryousato 0:5637e966d2f5 535 //パラメータがオン時のパルス幅(****us)のとき
ryousato 0:5637e966d2f5 536 else {
ryousato 0:5637e966d2f5 537 int tmpInt = atoi(para.c_str());
ryousato 0:5637e966d2f5 538 int pulseus = off_width;
ryousato 0:5637e966d2f5 539 servopwm.period_ms(20); //PWMサイクル周期(サーボモータSG92仕様)
ryousato 0:5637e966d2f5 540 while (pulseus >= tmpInt) {
ryousato 0:5637e966d2f5 541 servopwm.pulsewidth_us(pulseus);
ryousato 0:5637e966d2f5 542 wait_ms(waitms);
ryousato 0:5637e966d2f5 543 pulseus = pulseus - 1;
ryousato 0:5637e966d2f5 544 }
ryousato 0:5637e966d2f5 545
ryousato 0:5637e966d2f5 546 wait(1);
ryousato 0:5637e966d2f5 547
ryousato 0:5637e966d2f5 548 pulseus = tmpInt;
ryousato 0:5637e966d2f5 549 //servopwm.period_ms(20); //PWMサイクル周期(サーボモータSG92仕様)
ryousato 0:5637e966d2f5 550 while (pulseus <= off_width) {
ryousato 0:5637e966d2f5 551 servopwm.pulsewidth_us(pulseus);
ryousato 0:5637e966d2f5 552 wait_ms(waitms);
ryousato 0:5637e966d2f5 553 pulseus = pulseus + 1;
ryousato 0:5637e966d2f5 554 }
ryousato 0:5637e966d2f5 555 SerialCom.printf ("OK\r\n");
ryousato 0:5637e966d2f5 556 }
ryousato 0:5637e966d2f5 557 return 0;
ryousato 0:5637e966d2f5 558 }
ryousato 0:5637e966d2f5 559
ryousato 0:5637e966d2f5 560 //リレーとマルチプレクサの初期設定
ryousato 1:e5b0268067e4 561 int initial(std::string para)
ryousato 0:5637e966d2f5 562 {
ryousato 1:e5b0268067e4 563 //LEDチェック
ryousato 1:e5b0268067e4 564 led_all_check(none);
ryousato 1:e5b0268067e4 565
ryousato 0:5637e966d2f5 566 relayctl1 = 0;
ryousato 0:5637e966d2f5 567 relayctl2 = 0;
ryousato 0:5637e966d2f5 568 relayctl3 = 0;
ryousato 0:5637e966d2f5 569 relayctl4 = 0;
ryousato 0:5637e966d2f5 570 relayctl5 = 0;
ryousato 0:5637e966d2f5 571 relayctl6 = 0;
ryousato 0:5637e966d2f5 572 relayctl7 = 0;
ryousato 0:5637e966d2f5 573 relayctl8 = 0;
ryousato 0:5637e966d2f5 574 rmuxen = 1;
ryousato 0:5637e966d2f5 575 rmuxaddr.write(0);
ryousato 0:5637e966d2f5 576
ryousato 0:5637e966d2f5 577 //LEDのマルチプレクサをOFFに
ryousato 0:5637e966d2f5 578 cmuxen = 1;
ryousato 0:5637e966d2f5 579
ryousato 0:5637e966d2f5 580 return;
ryousato 0:5637e966d2f5 581 }
ryousato 0:5637e966d2f5 582
ryousato 0:5637e966d2f5 583 //LED全チェック
ryousato 0:5637e966d2f5 584 int led_all_check(std::string para)
ryousato 0:5637e966d2f5 585 {
ryousato 0:5637e966d2f5 586 //基板とレバーが未検知の時はコマンドを実行しない
ryousato 0:5637e966d2f5 587 int intdet0 = det0;
ryousato 0:5637e966d2f5 588 int intdet1 = det1;
ryousato 0:5637e966d2f5 589 int intdet2 = det2;
ryousato 0:5637e966d2f5 590 if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 591 SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
ryousato 0:5637e966d2f5 592 return 0;
ryousato 0:5637e966d2f5 593 }
ryousato 0:5637e966d2f5 594
ryousato 0:5637e966d2f5 595 char buf[2];
ryousato 0:5637e966d2f5 596
ryousato 0:5637e966d2f5 597 //led_lum_checkをLED1-24まで実施
ryousato 0:5637e966d2f5 598 for (int x = 1; x <= 3; x++) {
ryousato 0:5637e966d2f5 599 snprintf(buf, 12, "%d", x);
ryousato 0:5637e966d2f5 600 std::string str(buf, 2);
ryousato 0:5637e966d2f5 601 led_lum_check(str);
ryousato 0:5637e966d2f5 602 }
ryousato 0:5637e966d2f5 603 return 0;
ryousato 0:5637e966d2f5 604 }
ryousato 0:5637e966d2f5 605
ryousato 0:5637e966d2f5 606 void risedet_interrupt()
ryousato 0:5637e966d2f5 607 {
ryousato 0:5637e966d2f5 608 wait(0.001);
ryousato 0:5637e966d2f5 609 led = 0;
ryousato 0:5637e966d2f5 610 int intdet0 = det0;
ryousato 0:5637e966d2f5 611 int intdet1 = det1;
ryousato 0:5637e966d2f5 612 int intdet2 = det2;
ryousato 0:5637e966d2f5 613 if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 614 SerialCom.printf("\r\n Error! PKG, lever undetected! initializing...\r\n");
ryousato 0:5637e966d2f5 615
ryousato 0:5637e966d2f5 616 //初期設定
ryousato 1:e5b0268067e4 617 initial(none);
ryousato 0:5637e966d2f5 618
ryousato 0:5637e966d2f5 619 //サーボがonの場合はoffへ
ryousato 0:5637e966d2f5 620 if(servoflag == 1) {
ryousato 0:5637e966d2f5 621 servo_turn("off");
ryousato 0:5637e966d2f5 622 }
ryousato 0:5637e966d2f5 623
ryousato 0:5637e966d2f5 624 //PKGとレバーが検知されるのを待つ
ryousato 0:5637e966d2f5 625 SerialCom.printf ("\r\n Waiting for PKG installation...\r\n");
ryousato 0:5637e966d2f5 626 intdet0 = det0;
ryousato 0:5637e966d2f5 627 intdet1 = det1;
ryousato 0:5637e966d2f5 628 intdet2 = det2;
ryousato 0:5637e966d2f5 629 while(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 630 intdet0 = det0;
ryousato 0:5637e966d2f5 631 intdet1 = det1;
ryousato 0:5637e966d2f5 632 intdet2 = det2;
ryousato 0:5637e966d2f5 633 if (SerialCom.readable()) { //読み込む文字があるかどうか判定
ryousato 0:5637e966d2f5 634 main_uart_receive();
ryousato 0:5637e966d2f5 635 }
ryousato 0:5637e966d2f5 636 }
ryousato 0:5637e966d2f5 637 SerialCom.printf("\r\n PKG, lever detected!\r\n\r\n restart!\r\n\r\n");
ryousato 0:5637e966d2f5 638 }
ryousato 0:5637e966d2f5 639 led = 1;
ryousato 0:5637e966d2f5 640 }
ryousato 0:5637e966d2f5 641
ryousato 0:5637e966d2f5 642 int main()
ryousato 0:5637e966d2f5 643 {
ryousato 0:5637e966d2f5 644 led = 0;
ryousato 0:5637e966d2f5 645
ryousato 0:5637e966d2f5 646 //UART ボーレート設定
ryousato 0:5637e966d2f5 647 SerialCom.baud(SirialComSpeed);
ryousato 0:5637e966d2f5 648
ryousato 0:5637e966d2f5 649 //起動表示
ryousato 0:5637e966d2f5 650 SerialCom.printf ("PCU-1L Unitchecker Initializing...\r\n");
ryousato 0:5637e966d2f5 651
ryousato 0:5637e966d2f5 652 /* 基板上にプルアップを入れたのでコメントアウト
ryousato 0:5637e966d2f5 653 //ピン変化割込みポートモード設定
ryousato 0:5637e966d2f5 654 det0.mode(PullUp);
ryousato 0:5637e966d2f5 655 det1.mode(PullUp);
ryousato 0:5637e966d2f5 656 det2.mode(PullUp);
ryousato 0:5637e966d2f5 657 */
ryousato 0:5637e966d2f5 658
ryousato 0:5637e966d2f5 659 //初期設定
ryousato 1:e5b0268067e4 660 initial(none);
ryousato 0:5637e966d2f5 661
ryousato 0:5637e966d2f5 662 //サーボモータ第一初期化
ryousato 0:5637e966d2f5 663 pservoflag = &servoflag; //ポインター(サーボホーン位置情報格納用)
ryousato 0:5637e966d2f5 664 poff_width = &off_width;
ryousato 0:5637e966d2f5 665 pon_width = &on_width;
ryousato 0:5637e966d2f5 666 servopwm.period_ms(20); //PWMサイクル周期(サーボモータSG92仕様)
ryousato 0:5637e966d2f5 667 servopwm.pulsewidth_us(off_width); //角度が決まる
ryousato 0:5637e966d2f5 668
ryousato 0:5637e966d2f5 669 //PKGとレバーが検知されるのを待つ
ryousato 0:5637e966d2f5 670 int intdet0 = det0;
ryousato 0:5637e966d2f5 671 int intdet1 = det1;
ryousato 0:5637e966d2f5 672 int intdet2 = det2;
ryousato 0:5637e966d2f5 673 if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 674 SerialCom.printf ("\r\n Waiting for PKG installation...\r\n");
ryousato 0:5637e966d2f5 675 while(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
ryousato 0:5637e966d2f5 676 //wait(1);
ryousato 0:5637e966d2f5 677 intdet0 = det0;
ryousato 0:5637e966d2f5 678 intdet1 = det1;
ryousato 0:5637e966d2f5 679 intdet2 = det2;
ryousato 0:5637e966d2f5 680 if (SerialCom.readable()) { //読み込む文字があるかどうか判定
ryousato 0:5637e966d2f5 681 main_uart_receive();
ryousato 0:5637e966d2f5 682 }
ryousato 0:5637e966d2f5 683 }
ryousato 0:5637e966d2f5 684 SerialCom.printf("\r\n PKG, lever detected!\r\n\r\n");
ryousato 0:5637e966d2f5 685 }
ryousato 0:5637e966d2f5 686
ryousato 0:5637e966d2f5 687 led = 1;
ryousato 0:5637e966d2f5 688
ryousato 0:5637e966d2f5 689 //PKG,レバー未検知割込みを起動
ryousato 0:5637e966d2f5 690 det0.rise(&risedet_interrupt);
ryousato 0:5637e966d2f5 691 det1.rise(&risedet_interrupt);
ryousato 0:5637e966d2f5 692 det2.rise(&risedet_interrupt);
ryousato 0:5637e966d2f5 693
ryousato 0:5637e966d2f5 694 SerialCom.printf (" ready!\r\n\r\n");
ryousato 0:5637e966d2f5 695 //メインループ
ryousato 0:5637e966d2f5 696 while(1) {
ryousato 0:5637e966d2f5 697 if (SerialCom.readable()) { //読み込む文字があるかどうか判定
ryousato 0:5637e966d2f5 698 main_uart_receive();
ryousato 0:5637e966d2f5 699 }
ryousato 0:5637e966d2f5 700 }
ryousato 0:5637e966d2f5 701 }