SS2
2号機
Fork of
Nucleo_SS2_No2
by 
Rui Kato
Diff: main.cpp
- Revision:
- 6:d264f7f2b781
- Parent:
- 5:ab74616c24b4
--- a/main.cpp Tue Dec 12 00:49:19 2017 +0000
+++ b/main.cpp Tue Dec 19 01:23:44 2017 +0000
@@ -1,23 +1,29 @@
#include "mbed.h"
#include "hcsr04.h"
-PwmOut mypwm4(D4);
-PwmOut mypwm6(D6);
-PwmOut mypwm2(D2);
-PwmOut mypwm3(D3);
+AnalogIn photoref_L(A1); // フォトリフレクタ左
+AnalogIn photoref_M(A2); // フォトリフレクタ中央
+AnalogIn photoref_R(A3); // フォトリフレクタ右
-Ticker flag;
-Ticker timerflag;
+PwmOut mypwm4(D4); // モータ
+PwmOut mypwm6(D6); // モータ
+PwmOut mypwm2(D2); // モータ
+PwmOut mypwm3(D3); // モータ
+
+HCSR04 usensor1(D9,D10); // 超音波後；Trigger(DO), Echo(PWMIN)
+HCSR04 usensor2(D11,D12); // 超音波横；Trigger(DO), Echo(PWMIN)
+
Timer t;
+Ticker timer_flag;
+
DigitalOut myled(LED1);
-HCSR04 usensor1(D9,D10); // Trigger(DO), Echo(PWMIN)
-HCSR04 usensor2(D11,D12); // Trigger(DO), Echo(PWMIN)
+
unsigned int dist1,dist2;
-#define STATE_A (0)
+#define STATE_A (0) // 前進
#define STATE_B (1)
#define STATE_C (2)
#define STATE_D (3)
@@ -25,15 +31,18 @@
#define STATE_A2 (5)
#define STATE_A3 (6)
-
-char mode,fase;
+char mode = STATE_E;
+int fase;
int wait_flag;
int timer;
+void LineTrace(void);
+float val_L, val_R, val_M;
-void tflg(){
- timer = t.read();
- }
+
+void tflg() {
+ timer = t.read();
+}
/*void flg(){
usensor.start();
dist=usensor.get_dist_cm();
@@ -41,205 +50,222 @@
}*/
int main() {
+ t.start();
+ wait(3);
+ wait_flag = 0;
+ fase = 0;
-t.start();
-mode = STATE_E;
-wait_flag = 0;
-
-fase = 0;
-
-mypwm4.period_ms(100);
-mypwm6.period_ms(100);
-mypwm2.period_ms(100);
-mypwm3.period_ms(100);
+ timer_flag.attach(&tflg, 1.0); // タイマー割り込み設定
+
+ while(1) {
+ val_M = photoref_M.read();
+ val_L = photoref_L.read();
+ val_R = photoref_R.read();
+ printf("M: %f\n", val_M);
+ printf("L: %f\n", val_L);
+ printf("R: %f\n", val_R);
-timerflag.attach(&tflg, 1.0);
-/*flag.attach(&flg, 1.0);*/
-
-
-while(1) {
-usensor1.start();
-usensor2.start();
-wait(0.2);
-dist1=usensor1.get_dist_cm();
-dist2=usensor2.get_dist_cm();
-/*printf("dist1: %ld\n",dist1); */
-printf("dist2: %ld\n",dist2);
-
-
- /* switch(timer)
-{
- case 8.0:
- wait_flag = 1;
- mode = STATE_A;
- break;
- case 12.0:
- wait_flag = 1;
- mode = STATE_B;
- break;
- case 15.5:
- wait_flag = 1;
- mode = STATE_A2;
- break;
- case 22.0:
- wait_flag = 1;
- mode = STATE_D;
- break;
- case 30.0:
- wait_flag = 1;
- mode = STATE_A;
- break;
- case 37.0:
- wait_flag = 1;
- mode = STATE_C;
- break;
- case 42.0:
- wait_flag = 1;
- mode = STATE_A;
- break;
+ if(val_L > 0.5 && val_R > 0.5) { // 左：白，右：白
+ LineTrace(); // 直進
+ } else if(val_L < 0.5 && val_R > 0.5) { // 左：白，右：黒
+ mode = STATE_A2; // 右前進
+ } else if(val_L > 0.5 && val_R < 0.5) { // 左：黒，右：白
+ mode = STATE_A3; // 左前進
+ } else { // 左：黒，右：黒
+ mode = STATE_E; // ブレーキ
+ }
+ wait_ms(100);
+ /*
+ // printf("timer: %d\n", timer);
-}*/
-
-switch (fase)
- {
+ // 超音波取得
+ usensor1.start();
+ usensor2.start();
+ wait_ms(500);
+ dist1 = usensor1.get_dist_cm();
+ dist2 = usensor2.get_dist_cm();
+ printf("dist1: %ld [cm] ",dist1);
+ printf("dist2: %ld [cm]\n",dist2);
+
+ switch(fase) {
case 0:
- /*printf("timer: %d\n", timer);*/
- if(timer > 5){
+ mode = STATE_A; // 前進
+ //if(timer > 5) {
/*if(dist2 > 8){
mode = STATE_A3; //hidariに
}else if(dist2 < 10){
mode = STATE_A2; //migiに
}else{mode = STATE_A;}
- }*/if(dist1 > 45 && dist1<250){
+ }
+ if(dist1 > 41 && dist1 < 250) { // 後ろの壁から45cmの地点
+ wait_flag = 1;
fase = 1;
t.stop();
t.reset();
t.start();
}
- break;
+ break;
case 1:
- if(timer == 0){
- wait_flag = 1;
- mode = STATE_B;
- }else if(timer > 2 && dist1 > 60){
+ mode = STATE_B; // 右旋回
+ if(timer > 2) {
wait_flag = 1;
- mode = STATE_A;
+ fase = 2;
t.reset();
- t.stop();
- fase = 2;
+ t.stop();
t.start();
- }
+ }
break;
case 2:
- if(timer == 6.0){
+ mode = STATE_A; // 前進
+ if(timer > 4.0) {
+ wait_flag = 1;
+ fase = 3;
+ }
+ break;
+ case 3:
+ mode = STATE_D;
+ if(timer > 13.0) {
+ wait_flag = 1;
+ fase = 4;
+ }
+ break;
+ case 4:
+ mode = STATE_A; // 前進
+ if(timer > 21.0) {
wait_flag = 1;
- mode = STATE_D;
- }else if(timer == 16.0){
- wait_flag = 1;
- mode = STATE_A;
- }else if(timer == 26.0){
- wait_flag = 1;
- mode = STATE_D;
- }else if(timer == 36.0){
- wait_flag = 1;
- mode = STATE_A;
- }else if(timer == 46.0){
- wait_flag = 1;
- mode = STATE_D;}
+ fase = 5;
+ }
+ break;
+ case 5:
+ mode = STATE_D; // 後退
+ if(timer > 29.0) {
+ wait_flag = 1;
+ fase = 6;
+ }
+ break;
+ case 6:
+ mode = STATE_A; // 前進
+ if(timer > 37.0) {
+ wait_flag = 1;
+ fase = 7;
+ }
+ break;
+ case 7:
+ mode = STATE_D; // 後退
+ if(timer > 37) {
+ mode = STATE_E; // 停止
+ }
+ break;
+ default:
+ break;
}
-
-// 状態が切り替わるときは一時停止
- if(wait_flag==1)
- {
+
+ // 状態が切り替わるときは一時停止
+ if(wait_flag == 1) {
// フラグの初期化
wait_flag = 0;
myled = 0; // LED消灯
// 左モータの制御
- mypwm4.pulsewidth_ms(80);
- mypwm6.pulsewidth_ms(80);
+ mypwm4.pulsewidth_ms(50);
+ mypwm6.pulsewidth_ms(50);
// 右モータの制御
- mypwm2.pulsewidth_ms(80);
- mypwm3.pulsewidth_ms(80);
+ mypwm2.pulsewidth_ms(50);
+ mypwm3.pulsewidth_ms(50);
// 500ms待機する
+ mode = STATE_E;
wait(0.5);
}
-
-switch (mode)
- {
+ */
+ switch(mode) {
// STATE_A : 前進（左：正転 右：正転）
case STATE_A:
+ printf("STATE_A\n");
myled = 1; // LED点灯
// 左モータの制御
- mypwm4.pulsewidth_ms(70);
+ mypwm4.pulsewidth_ms(15);
mypwm6.pulsewidth_ms(0);
// 右モータの制御
- mypwm2.pulsewidth_ms(70);
+ mypwm2.pulsewidth_ms(15);
mypwm3.pulsewidth_ms(0);
break;
- // STATE_A2 : 右に曲がる（左：正転 右：正転）
+ // STATE_A2 : 右に曲がる（左：正転 右：正転）
case STATE_A2:
+ printf("STATE_A2\n");
myled = 1; // LED点灯
// 左モータの制御
- mypwm4.pulsewidth_ms(50);
+ mypwm4.pulsewidth_ms(10);
mypwm6.pulsewidth_ms(0);
// 右モータの制御
- mypwm2.pulsewidth_ms(80);
+ mypwm2.pulsewidth_ms(6);
mypwm3.pulsewidth_ms(0);
break;
- // STATE_A3 : hidariに曲がる（左：正転 右：正転）
+ // STATE_A3 : hidariに曲がる（左：正転 右：正転）
case STATE_A3:
+ printf("STATE_A3\n");
myled = 1; // LED点灯
// 左モータの制御
- mypwm4.pulsewidth_ms(80);
+ mypwm4.pulsewidth_ms(6);
mypwm6.pulsewidth_ms(0);
// 右モータの制御
mypwm2.pulsewidth_ms(10);
mypwm3.pulsewidth_ms(0);
break;
-
// STATE_B : 右旋回（左：正転 右：逆転）
case STATE_B:
+ printf("STATE_B\n");
myled = 1; // LED点灯
// 左モータの制御
- mypwm4.pulsewidth_ms(100);
+ mypwm4.pulsewidth_ms(15);
mypwm6.pulsewidth_ms(0);
// 右モータの制御
mypwm2.pulsewidth_ms(0);
- mypwm3.pulsewidth_ms(100);
+ mypwm3.pulsewidth_ms(15);
break;
-
// STATE_C : 左旋回（左：逆転 右：正転）
case STATE_C:
+ printf("STATE_C\n");
+ myled = 1; // LED点灯
+ // 左モータの制御
+ mypwm4.pulsewidth_ms(0);
+ mypwm6.pulsewidth_ms(15);
+ // 右モータの制御
+ mypwm2.pulsewidth_ms(15);
+ mypwm3.pulsewidth_ms(0);
+ break;
+ // STATE_D : 後退（左：逆転 右：逆転）
+ case STATE_D:
+ printf("STATE_D\n");
myled = 1; // LED点灯
// 左モータの制御
mypwm4.pulsewidth_ms(0);
- mypwm6.pulsewidth_ms(100);
- // 右モータの制御
- mypwm2.pulsewidth_ms(100);
- mypwm3.pulsewidth_ms(0);
- break;
-
- // STATE_D : 後退（左：逆転 右：逆転）
- case STATE_D:
- myled = 1; // LED点灯
- // 左モータの制御
- mypwm4.pulsewidth_ms(0);
- mypwm6.pulsewidth_ms(80);
+ mypwm6.pulsewidth_ms(15);
// 右モータの制御
mypwm2.pulsewidth_ms(0);
- mypwm3.pulsewidth_ms(83);
+ mypwm3.pulsewidth_ms(15);
break;
-
// STATE_E : ブレーキ（左：ブレーキ 右：ブレーキ）
case STATE_E:
+ printf("STATE_E\n");
myled = 0; // LED消灯
// 左モータの制御
- mypwm4.pulsewidth_ms(60);
- mypwm6.pulsewidth_ms(60);
+ mypwm4.pulsewidth_ms(30);
+ mypwm6.pulsewidth_ms(30);
// 右モータの制御
- mypwm2.pulsewidth_ms(60);
- mypwm3.pulsewidth_ms(60);
+ mypwm2.pulsewidth_ms(30);
+ mypwm3.pulsewidth_ms(30);
+ break;
+ default:
break;
}
}
-} }
+}
+
+
+void LineTrace(void) { // ラインの右側でライントレース
+ if(val_M > 0.5) { // 中央：白
+ mode = STATE_A2; // 左前進
+ } else { // 中央：黒
+ mode = STATE_A3; // 右前進
+ }
+ wait_ms(50);
+}
\ No newline at end of file