mbed_robotcar
赤子を殺すより楽な作業よ
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 5:5414f85108aa
- Parent:
- 4:3f80c0180e2f
- Child:
- 6:4817fe7b7654
--- a/main.cpp Wed Jul 29 03:10:44 2020 +0000
+++ b/main.cpp Wed Jul 29 06:36:04 2020 +0000
@@ -55,25 +55,26 @@
int mode; // 操作モード
int run; // 走行状態
int beforeMode; // 前のモード
-int flag_sp = 0; // スピード変化フラグ
+int flag_sp = 0; // スピード変化フラグ
Timer viewTimer; // スピ―ド変更時に3秒計測
float motorSpeed[6] = {0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
// モーター速度設定(後半はライントレース用)
-Mutex mutex;
+
+Mutex mutex; // ミューテックス
+
// ポートp15を赤外線受信モジュールの接続先に指定
RemoteIR::Format format;
uint8_t buf[32];
uint32_t bitcount;
uint32_t code;
-//bChangeのいろいろ
+/* bChange, lcdBacklight用変数設定 */
TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
-int b = 0;
-double test;
-int flag_b = 0;
-int flag_t = 0;
+int b = 0; // バッテリー残量
+int flag_b = 0; // バックライト点滅フラグ
+int flag_t = 0; // バックライトタイマーフラグ
-// ライントレース
+/* ライントレース */
int sensArray[32] = {0,0,0,0,0,0,0,0, // ライントレースセンサパターン
0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,
@@ -87,7 +88,7 @@
int SR; // 右
int SLD; // 左前
int SRD; // 右前
-int flag = 0; // 障害物有無のフラグ
+int flag_a = 0; // 障害物有無のフラグ
const int limit = 20; // 障害物の距離のリミット(単位：cm)
int far; // 最も遠い距離
int houkou; // 進行方向(1:前 2:左 3:右)
@@ -103,17 +104,20 @@
void trace(void const *argument);
void watchsurrounding();
int watch();
-void bChange(/*void const *argument*/);
+void bChange();
void display();
void lcdBacklight(void const *argument);
+
+/* スレッド宣言 */
Thread thread1(decodeIR, NULL, osPriorityRealtime);//+3
Thread thread2(motor, NULL, osPriorityHigh);//+2
Thread thread3(avoidance, NULL, osPriorityHigh);//+2
Thread thread4(trace, NULL, osPriorityHigh);//+2
-//Thread thread5(bChange, NULL, osPriorityBelowNormal);//-1
+
+/* タイマー割り込み */
RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
-
+/* リモコン受信スレッド */
void decodeIR(void const *argument){
while(1){
// 受信待ち
@@ -130,8 +134,6 @@
if(mode!=SPEED){
beforeMode=mode;
}
-
- //pc.printf("%0x\r\n",code);
switch(code){
case 0x40bf27d8://クイック
pc.printf("mode = SPEED\r\n");
@@ -141,7 +143,7 @@
mode = beforeMode;
break;
case 0x40be34cb://レグザリンク
- pc.printf("mode = INE_TRACE\r\n");
+ pc.printf("mode = LINE_TRACE\r\n");
mode=LINE_TRACE;
display();
break;
@@ -186,22 +188,17 @@
}
}
if(viewTimer.read_ms()>=3000){
- //pc.printf("a");
viewTimer.stop();
- //defaultView();
viewTimer.reset();
display();
- //flaga=0;
}
- //pc.printf(" main2\r\n");
- //if(viewTimer.read_ms()==0){
- // display();
- //}
ThisThread::sleep_for(90);
}
}
+
+/* モータースレッド */
void motor(void const *argument){
- while(true){
+ while(1){
pc.printf("motor\r\n");
if(flag_sp >VERYFAST){
flag_sp -= 3;
@@ -242,16 +239,20 @@
ThisThread::sleep_for(30);
}
}
+
+/* スピード変更関数 */
void changeSpeed(){
if(flag_sp == VERYFAST){
- flag_sp = NORMAL;
-
- }else{
+ flag_sp = NORMAL;
+ }
+ else{
flag_sp = flag_sp + 1;
}
}
+
+/* ライントレーススレッド */
void trace(void const *argument){
- while(true){
+ while(1){
if(mode==LINE_TRACE){
pc.printf("line trace\r\n");
// センサー値読み取り
@@ -299,15 +300,15 @@
}
}
-/* 障害物回避走行 */
+/* 障害物回避走行スレッド */
void avoidance(void const *argument){
while(1){
if(mode==AVOIDANCE){
pc.printf("avoidance\r\n");
- if(flag == 0){ // 障害物がない場合
+ if(flag_a == 0){ // 障害物がない場合
run = ADVANCE; // 前進
}
- /*else{ // 障害物がある場合
+ else{ // 障害物がある場合
i = 0;
if(SC < 15){ // 正面15cm以内に障害物が現れた場合
run = BACK; // 後退
@@ -318,7 +319,7 @@
}
if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
run = LEFT; // 左折
- while(i < 100){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
if(watch() > limit){
i++;
}
@@ -356,7 +357,7 @@
break;
case 2: // 左の場合
run = LEFT; // 左折
- while(i < 100){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
if(watch() > (far - 2)){
i++;
}
@@ -368,7 +369,7 @@
break;
case 3: // 右の場合
run = RIGHT; // 右折
- while(i < 100){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
if(watch() > (far - 2)){
i++;
}
@@ -380,8 +381,8 @@
break;
}
}
- }*/
- flag = 0; // フラグを0にセット
+ }
+ flag_a = 0; // 障害物有無フラグを0にセット
watchsurrounding();
pc.printf(" avoidance\r\n");
}else{
@@ -389,6 +390,7 @@
}
}
}
+
int watch(){
pc.printf("watch\r\n");
trig = 0;
@@ -452,47 +454,33 @@
pc.printf(" watchsurrounding\r\n");
}
-void defaultView(){
- lcd.setAddress(0,0);
- lcd.printf("Battery:");
- lcd.setAddress(0,1);
- lcd.printf("Mode:");
-}
-
+/* ディスプレイ表示関数 */
void display(){
mutex.lock();
lcd.setAddress(0,1);
switch(mode){
case ADVANCE:
- //strcpy(DispMode,"Mode:Advance");
lcd.printf("Mode:Advance ");
break;
case RIGHT:
- //strcpy(DispMode,"Mode:Right");
- lcd.printf("Mode:Right ");
+ lcd.printf("Mode:TurnRight ");
break;
case LEFT:
- //strcpy(DispMode,"Mode:Left");
- lcd.printf("Mode:Left ");
+ lcd.printf("Mode:TurnLeft ");
break;
case BACK:
- //strcpy(DispMode,"Mode:Back");
lcd.printf("Mode:Back ");
break;
case STOP:
- //strcpy(DispMode,"Mode:Stop");
lcd.printf("Mode:Stop ");
break;
case READY:
- //strcpy(DispMode,"Mode:Stop");
lcd.printf("Mode:Ready ");
break;
case LINE_TRACE:
- //strcpy(DispMode,"Mode:LineTrace");
lcd.printf("Mode:LineTrace ");
break;
case AVOIDANCE:
- //strcpy(DispMode,"Mode:Avoidance");
lcd.setAddress(0,1);
lcd.printf("Mode:Avoidance ");
break;
@@ -524,23 +512,7 @@
flag_b = !flag_b;
}
-void bChange(/*void const *argument*/){
- /*while(1){
- pc.printf(" bChange1\r\n");
- lcd.setBacklight(TextLCD::LightOn);
- test = battery.read()*MAX_V;
- b = (int)((test - MIN_V)/0.107 + 0.5)*10;
- if(b < 0){
- b = 0;
- }else if(b > 100){
- b = 100;
- }
- mutex.lock();
- lcd.setAddress(0,0);
- lcd.printf("Battery:%3d%% ",b);
- pc.printf(" bChange2\r\n");
- mutex.unlock();
- }*/
+void bChange(){
lcd.setBacklight(TextLCD::LightOn);
while(1){
pc.printf(" bChange1\r\n");
@@ -551,7 +523,8 @@
bTimer.stop();
exit(1);*/
b = 0;
- }else if(b > 100){
+ }
+ else if(b > 100){
b = 100;
}
mutex.lock();
@@ -564,7 +537,8 @@
bTimer.start(500);
flag_t = 1;
}
- }else{
+ }
+ else{
if(flag_t == 1){
bTimer.stop();
lcd.setBacklight(TextLCD::LightOn);
@@ -574,54 +548,15 @@
}
ThisThread::sleep_for(500);
}
+
int main() {
mode = READY;
beforeMode = READY;
run = STOP;
flag_sp = NORMAL;
display();
- //defaultView();
-
while(1){
pc.printf(" main1\r\n");
- // 受信待ち
- /*switch(mode){
- case ADVANCE:
- run = ADVANCE;
- break;
- case RIGHT:
- run = RIGHT;
- break;
- case LEFT:
- run = LEFT;
- break;
- case BACK:
- run = BACK;
- break;
- case STOP:
- run = STOP;
- break;
- case READY:
- run = STOP;
- break;
- case SPEED:
- changeSpeed();
- display();
- mode = beforeMode;
- break;
- }*/
-
-/* if(viewTimer.read_ms()>=3000){
- pc.printf("a");
- viewTimer.stop();
- //defaultView();
- viewTimer.reset();
- //flaga=0;
- }
- pc.printf(" main2\r\n");
- if(viewTimer.read_ms()==0){
- display();
- }*/
bChange();
pc.printf(" main3\r\n");
ThisThread::sleep_for(1);