siqiang pan
ボタンを押すと、 バッテリ更新を停止し、 他のボタンもロックさせる
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 2:38825726cb1b
- Parent:
- 1:5bb497a38344
- Child:
- 3:2ae6218973be
--- a/main.cpp Wed Jul 22 07:43:56 2020 +0000
+++ b/main.cpp Mon Jul 27 08:49:28 2020 +0000
@@ -8,11 +8,18 @@
#include "rtos.h"
#include <stdint.h>
#include "platform/mbed_thread.h"
+#include "TextLCD.h"
+#define MIN_V 2.23
+#define MAX_V 3.3
+
+//bChangeのいろいろ
+I2C i2c_lcd(p9, p10);
+TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
+int b = 0;
+AnalogIn battery(p15);
+double test;
Serial pc(USBTX, USBRX);
-
-
-DigitalIn button(p15);
// ポートp15を赤外線受信モジュールの接続先に指定
ReceiverIR ir_rx(p15);
RemoteIR::Format format;
@@ -20,11 +27,53 @@
uint32_t bitcount;
uint32_t code;
-void rimokon();
+//障害物回避の設定
+DigitalOut trig(p6); // 超音波センサのtrigピンをp6に接続
+DigitalIn echo(p7); // 超音波センサのechoピンをp7に接続
+PwmOut servo(p25); // サーボコントロールピン(p25)
+
+enum Mode{
+ ADVANCE,
+ RIGHT,
+ LEFT,
+ BACK,
+ STOP,
+ LINE_TRACE,
+ AVOIDANCE,
+ READY
+};
+
+Mode run;
+Mode mode;
+
+Timer timer; // 距離計測用タイマー
-void rimokon(/*void const *argument*/){
-
-
+/* 障害物検知用の変数設定 */
+int SC; // 正面
+int SL; // 左
+int SR; // 右
+int SLD; // 左前
+int SRD; // 右前
+int flag = 0; // 障害物有無のフラグ
+const int limit = 20; // 障害物の距離のリミット(単位：cm)
+int DT; // 距離
+int houkou; // 進行方向(1:前 2:左 3:右)
+int far; // 最も遠い距離
+int i; // ループ変数
+
+void rimokon(void const *argument);
+void avoidance(void const *argument);
+void motor(void const *argument);
+void watchsurrounding();
+int watch();
+void bChange(void const *argument);
+Thread thread1(rimokon, NULL, osPriorityRealtime);
+Thread thread2(motor, NULL, osPriorityHigh);
+Thread thread3(avoidance, NULL, osPriorityAboveNormal);
+Thread thread4(bChange, NULL, osPriorityBelowNormal);
+
+void rimokon(void const *argument){
+ while(1){
// 受信待ち
if (ir_rx.getState() == ReceiverIR::Received) {
// コード受信
@@ -132,9 +181,11 @@
pc.printf("↓\r\n");
break;
case 0x40bf5fa0://←
+ mode=LINE_TRACE;
pc.printf("←\r\n");
break;
case 0x40bf5ba4://→
+ mode=AVOIDANCE;
pc.printf("→\r\n");
break;
case 0x40bf3dc2://決定
@@ -171,17 +222,184 @@
}
}
}
-
+ ThisThread::sleep_for(10);
+ }
+}
+void motor(void const *argument){
+ while(1){
+ pc.printf("motor\r\n");
+ ThisThread::sleep_for(20);
+ }
+}
+/* 障害物回避走行 */
+void avoidance(void const *argument){
+
+ while(1){
+ if(mode==AVOIDANCE){
+ pc.printf("avoidance\r\n");
+ if(flag == 0){ // 障害物がない場合
+ run = ADVANCE; // 前進
+ }
+ /* else{ // 障害物がある場合
+ i = 0;
+ if(SC < 15){ // 正面15cm以内に障害物が現れた場合
+ run = BACK; // 後退
+ while(watch() < limit){ // 正面20cm以内に障害物がある間
+
+ }
+ run = STOP; // 停止
+ }
+ if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
+ run = LEFT; // 左折
+ while(i < 100){ // 進行方向確認
+ if(watch() > limit){
+ i++;
+ }
+ else{
+ i = 0;
+ }
+ }
+ run = STOP; // 停止
+ }
+ else { // 全方向以外
+ far = SC; // 正面を最も遠い距離に設定
+ houkou = 1; // 進行方向を前に設定
+ if(far < SLD || far < SL){ // 左または左前がより遠い場合
+ if(SL < SLD){ // 左前が左より遠い場合
+ far = SLD; // 左前を最も遠い距離に設定
+ }
+ else{ // 左が左前より遠い場合
+ far = SL; // 左を最も遠い距離に設定
+ }
+ houkou = 2; // 進行方向を左に設定
+ }
+ if(far < SRD || far < SR){ // 右または右前がより遠い場合
+ if(SR < SRD){ // 右前が右より遠い場合
+ far = SRD; // 右前を最も遠い距離に設定
+ }
+ else{ // 右が右前よりも遠い場合
+ far = SR; // 右を最も遠い距離に設定
+ }
+ houkou = 3; // 進行方向を右に設定
+ }
+ switch(houkou){ // 進行方向の場合分け
+ case 1: // 前の場合
+ run = ADVANCE; // 前進
+ thread_sleep_for(100); // 1秒待つ
+ break;
+ case 2: // 左の場合
+ run = LEFT; // 左折
+ while(i < 100){ // 進行方向確認
+ if(watch() > (far - 2)){
+ i++;
+ }
+ else{
+ i = 0;
+ }
+ }
+ run = STOP; // 停止
+ break;
+ case 3: // 右の場合
+ run = RIGHT; // 右折
+ while(i < 100){ // 進行方向確認
+ if(watch() > (far - 2)){
+ i++;
+ }
+ else{
+ i = 0;
+ }
+ }
+ run = STOP; // 停止
+ break;
+ }
+ }
+ }*/
+ flag = 0; // フラグを0にセット
+ watchsurrounding();
+ }
+ }
+}
+int watch(){
+ pc.printf("watch\r\n");
+ trig = 0;
+ ThisThread::sleep_for(5); // 5ms待つ
+ trig = 1;
+ ThisThread::sleep_for(15); // 15ms待つ
+ trig = 0;
+ //while(echo.read() == 0){
+ //}
+ timer.start(); // 距離計測タイマースタート
+ while(echo.read() == 1){
+ }
+ timer.stop(); // 距離計測タイマーストップ
+ DT = timer.read_us()*0.01657; // 距離計算
+ if(DT > 400){ // 検知範囲外なら400cmに設定
+ DT = 400;
+ }
+ timer.reset(); // 距離計測タイマーリセット
+ ThisThread::sleep_for(30); // 30ms待つ
+ return DT;
}
+void watchsurrounding(){
+ pc.printf("watchsurrounding\r\n");
+ SC = watch();
+ if(SC < limit){ // 正面20cm以内に障害物がある場合
+ run = STOP; // 停止
+ }
+ servo.pulsewidth_us(1925); // サーボを左に40度回転
+ ThisThread::sleep_for(250); // 250ms待つ
+ SLD = watch();
+ if(SLD < limit){ // 左前20cm以内に障害物がある場合
+ run = STOP; // 停止
+ }
+ servo.pulsewidth_us(2400); // サーボを左に90度回転
+ ThisThread::sleep_for(250); // 250ms待つ
+ SL = watch();
+ if(SL < limit){ // 左20cm以内に障害物がある場合
+ run = STOP; // 停止
+ }
+ servo.pulsewidth_us(1450);
+ ThisThread::sleep_for(100);
+ servo.pulsewidth_us(925); // サーボを右に40度回転
+ ThisThread::sleep_for(250); // 250ms待つ
+ SRD = watch();
+ if(SRD < limit){ // 右前20cm以内に障害物がある場合
+ run = STOP; // 停止
+ }
+ servo.pulsewidth_us(500); // サーボを右に90度回転
+ ThisThread::sleep_for(250); // 250ms待つ
+ SR = watch();
+ if(SR < limit){ // 右20cm以内に障害物がある場合
+ run = STOP; // 停止
+ }
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ ThisThread::sleep_for(100); // 100ms待つ
+ if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
+ flag = 1; // フラグに1をセット
+ }
+}
+
+
+void bChange(void const *argument){
+ while(1){
+ //lcd.setBacklight(TextLCD::LightOn);
+ test = battery.read()*MAX_V;
+ b = (int)((test - MIN_V)/0.107 + 0.5)*10;
+ lcd.locate(0,0);
+ lcd.printf("Battery:%3d%%",b);
+ pc.printf("bChange\r\n");
+ }
+}
int main() {
- //RtosTimer rimokon_timer(rimokon, osTimerPeriodic, (void *)0); // set RTOS timer for sensor
- //rimokon_timer.start(10);
- //Thread thread1(rimokon , NULL , osPriorityHigh);
+
+
while(1){
- if(button.read()==0){
+ /*if(button.read()==0){
rimokon();
- }
+ } */
+ pc.printf("main\r\n");
+ ThisThread::sleep_for(40);
}
}
\ No newline at end of file