2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2_No2 by Rui Kato

Committer:
kato8
Date:
Tue Dec 19 01:23:44 2017 +0000
Revision:
6:d264f7f2b781
Parent:
5:ab74616c24b4
2??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Morimoto448 0:7e3b7c017977 1 #include "mbed.h"
Morimoto448 3:dd3b7c3c327b 2 #include "hcsr04.h"
Morimoto448 0:7e3b7c017977 3
kato8 6:d264f7f2b781 4 AnalogIn photoref_L(A1); // フォトリフレクタ左
kato8 6:d264f7f2b781 5 AnalogIn photoref_M(A2); // フォトリフレクタ中央
kato8 6:d264f7f2b781 6 AnalogIn photoref_R(A3); // フォトリフレクタ右
Morimoto448 0:7e3b7c017977 7
kato8 6:d264f7f2b781 8 PwmOut mypwm4(D4); // モータ
kato8 6:d264f7f2b781 9 PwmOut mypwm6(D6); // モータ
kato8 6:d264f7f2b781 10 PwmOut mypwm2(D2); // モータ
kato8 6:d264f7f2b781 11 PwmOut mypwm3(D3); // モータ
kato8 6:d264f7f2b781 12
kato8 6:d264f7f2b781 13 HCSR04 usensor1(D9,D10); // 超音波後;Trigger(DO), Echo(PWMIN)
kato8 6:d264f7f2b781 14 HCSR04 usensor2(D11,D12); // 超音波横;Trigger(DO), Echo(PWMIN)
kato8 6:d264f7f2b781 15
Morimoto448 1:aabe6887447c 16 Timer t;
kato8 6:d264f7f2b781 17 Ticker timer_flag;
kato8 6:d264f7f2b781 18
Morimoto448 0:7e3b7c017977 19
Morimoto448 0:7e3b7c017977 20 DigitalOut myled(LED1);
Morimoto448 0:7e3b7c017977 21
kato8 6:d264f7f2b781 22
Morimoto448 4:04ca6a211b85 23 unsigned int dist1,dist2;
Morimoto448 3:dd3b7c3c327b 24
Morimoto448 0:7e3b7c017977 25
kato8 6:d264f7f2b781 26 #define STATE_A (0) // 前進
Morimoto448 0:7e3b7c017977 27 #define STATE_B (1)
Morimoto448 0:7e3b7c017977 28 #define STATE_C (2)
Morimoto448 0:7e3b7c017977 29 #define STATE_D (3)
Morimoto448 0:7e3b7c017977 30 #define STATE_E (4)
ishi777 2:f568ec2e6955 31 #define STATE_A2 (5)
Morimoto448 4:04ca6a211b85 32 #define STATE_A3 (6)
Morimoto448 4:04ca6a211b85 33
kato8 6:d264f7f2b781 34 char mode = STATE_E;
kato8 6:d264f7f2b781 35 int fase;
Morimoto448 0:7e3b7c017977 36 int wait_flag;
Morimoto448 1:aabe6887447c 37 int timer;
kato8 6:d264f7f2b781 38 void LineTrace(void);
kato8 6:d264f7f2b781 39 float val_L, val_R, val_M;
Morimoto448 0:7e3b7c017977 40
Morimoto448 4:04ca6a211b85 41
kato8 6:d264f7f2b781 42
kato8 6:d264f7f2b781 43 void tflg() {
kato8 6:d264f7f2b781 44 timer = t.read();
kato8 6:d264f7f2b781 45 }
Morimoto448 3:dd3b7c3c327b 46 /*void flg(){
Morimoto448 3:dd3b7c3c327b 47 usensor.start();
Morimoto448 3:dd3b7c3c327b 48 dist=usensor.get_dist_cm();
Morimoto448 3:dd3b7c3c327b 49 printf("dist:%u\n", dist);
Morimoto448 3:dd3b7c3c327b 50 }*/
Morimoto448 0:7e3b7c017977 51
Morimoto448 0:7e3b7c017977 52 int main() {
kato8 6:d264f7f2b781 53 t.start();
kato8 6:d264f7f2b781 54 wait(3);
kato8 6:d264f7f2b781 55 wait_flag = 0;
kato8 6:d264f7f2b781 56 fase = 0;
Morimoto448 0:7e3b7c017977 57
kato8 6:d264f7f2b781 58 timer_flag.attach(&tflg, 1.0); // タイマー割り込み設定
kato8 6:d264f7f2b781 59
kato8 6:d264f7f2b781 60 while(1) {
kato8 6:d264f7f2b781 61 val_M = photoref_M.read();
kato8 6:d264f7f2b781 62 val_L = photoref_L.read();
kato8 6:d264f7f2b781 63 val_R = photoref_R.read();
kato8 6:d264f7f2b781 64 printf("M: %f\n", val_M);
kato8 6:d264f7f2b781 65 printf("L: %f\n", val_L);
kato8 6:d264f7f2b781 66 printf("R: %f\n", val_R);
Morimoto448 0:7e3b7c017977 67
kato8 6:d264f7f2b781 68 if(val_L > 0.5 && val_R > 0.5) { // 左:白,右:白
kato8 6:d264f7f2b781 69 LineTrace(); // 直進
kato8 6:d264f7f2b781 70 } else if(val_L < 0.5 && val_R > 0.5) { // 左:白,右:黒
kato8 6:d264f7f2b781 71 mode = STATE_A2; // 右前進
kato8 6:d264f7f2b781 72 } else if(val_L > 0.5 && val_R < 0.5) { // 左:黒,右:白
kato8 6:d264f7f2b781 73 mode = STATE_A3; // 左前進
kato8 6:d264f7f2b781 74 } else { // 左:黒,右:黒
kato8 6:d264f7f2b781 75 mode = STATE_E; // ブレーキ
kato8 6:d264f7f2b781 76 }
kato8 6:d264f7f2b781 77 wait_ms(100);
kato8 6:d264f7f2b781 78 /*
kato8 6:d264f7f2b781 79 // printf("timer: %d\n", timer);
Morimoto448 1:aabe6887447c 80
kato8 6:d264f7f2b781 81 // 超音波取得
kato8 6:d264f7f2b781 82 usensor1.start();
kato8 6:d264f7f2b781 83 usensor2.start();
kato8 6:d264f7f2b781 84 wait_ms(500);
kato8 6:d264f7f2b781 85 dist1 = usensor1.get_dist_cm();
kato8 6:d264f7f2b781 86 dist2 = usensor2.get_dist_cm();
kato8 6:d264f7f2b781 87 printf("dist1: %ld [cm] ",dist1);
kato8 6:d264f7f2b781 88 printf("dist2: %ld [cm]\n",dist2);
kato8 6:d264f7f2b781 89
kato8 6:d264f7f2b781 90 switch(fase) {
Morimoto448 3:dd3b7c3c327b 91 case 0:
kato8 6:d264f7f2b781 92 mode = STATE_A; // 前進
kato8 6:d264f7f2b781 93 //if(timer > 5) {
Morimoto448 4:04ca6a211b85 94 /*if(dist2 > 8){
Morimoto448 4:04ca6a211b85 95 mode = STATE_A3; //hidariに
Morimoto448 4:04ca6a211b85 96 }else if(dist2 < 10){
Morimoto448 4:04ca6a211b85 97 mode = STATE_A2; //migiに
Morimoto448 4:04ca6a211b85 98 }else{mode = STATE_A;}
kato8 6:d264f7f2b781 99 }
kato8 6:d264f7f2b781 100 if(dist1 > 41 && dist1 < 250) { // 後ろの壁から45cmの地点
kato8 6:d264f7f2b781 101 wait_flag = 1;
Morimoto448 4:04ca6a211b85 102 fase = 1;
Morimoto448 3:dd3b7c3c327b 103 t.stop();
Morimoto448 3:dd3b7c3c327b 104 t.reset();
Morimoto448 3:dd3b7c3c327b 105 t.start();
Morimoto448 3:dd3b7c3c327b 106 }
kato8 6:d264f7f2b781 107 break;
Morimoto448 3:dd3b7c3c327b 108 case 1:
kato8 6:d264f7f2b781 109 mode = STATE_B; // 右旋回
kato8 6:d264f7f2b781 110 if(timer > 2) {
Morimoto448 3:dd3b7c3c327b 111 wait_flag = 1;
kato8 6:d264f7f2b781 112 fase = 2;
Morimoto448 3:dd3b7c3c327b 113 t.reset();
kato8 6:d264f7f2b781 114 t.stop();
Morimoto448 3:dd3b7c3c327b 115 t.start();
kato8 6:d264f7f2b781 116 }
Morimoto448 3:dd3b7c3c327b 117 break;
Morimoto448 3:dd3b7c3c327b 118 case 2:
kato8 6:d264f7f2b781 119 mode = STATE_A; // 前進
kato8 6:d264f7f2b781 120 if(timer > 4.0) {
kato8 6:d264f7f2b781 121 wait_flag = 1;
kato8 6:d264f7f2b781 122 fase = 3;
kato8 6:d264f7f2b781 123 }
kato8 6:d264f7f2b781 124 break;
kato8 6:d264f7f2b781 125 case 3:
kato8 6:d264f7f2b781 126 mode = STATE_D;
kato8 6:d264f7f2b781 127 if(timer > 13.0) {
kato8 6:d264f7f2b781 128 wait_flag = 1;
kato8 6:d264f7f2b781 129 fase = 4;
kato8 6:d264f7f2b781 130 }
kato8 6:d264f7f2b781 131 break;
kato8 6:d264f7f2b781 132 case 4:
kato8 6:d264f7f2b781 133 mode = STATE_A; // 前進
kato8 6:d264f7f2b781 134 if(timer > 21.0) {
Morimoto448 3:dd3b7c3c327b 135 wait_flag = 1;
kato8 6:d264f7f2b781 136 fase = 5;
kato8 6:d264f7f2b781 137 }
kato8 6:d264f7f2b781 138 break;
kato8 6:d264f7f2b781 139 case 5:
kato8 6:d264f7f2b781 140 mode = STATE_D; // 後退
kato8 6:d264f7f2b781 141 if(timer > 29.0) {
kato8 6:d264f7f2b781 142 wait_flag = 1;
kato8 6:d264f7f2b781 143 fase = 6;
kato8 6:d264f7f2b781 144 }
kato8 6:d264f7f2b781 145 break;
kato8 6:d264f7f2b781 146 case 6:
kato8 6:d264f7f2b781 147 mode = STATE_A; // 前進
kato8 6:d264f7f2b781 148 if(timer > 37.0) {
kato8 6:d264f7f2b781 149 wait_flag = 1;
kato8 6:d264f7f2b781 150 fase = 7;
kato8 6:d264f7f2b781 151 }
kato8 6:d264f7f2b781 152 break;
kato8 6:d264f7f2b781 153 case 7:
kato8 6:d264f7f2b781 154 mode = STATE_D; // 後退
kato8 6:d264f7f2b781 155 if(timer > 37) {
kato8 6:d264f7f2b781 156 mode = STATE_E; // 停止
kato8 6:d264f7f2b781 157 }
kato8 6:d264f7f2b781 158 break;
kato8 6:d264f7f2b781 159 default:
kato8 6:d264f7f2b781 160 break;
Morimoto448 3:dd3b7c3c327b 161 }
kato8 6:d264f7f2b781 162
kato8 6:d264f7f2b781 163 // 状態が切り替わるときは一時停止
kato8 6:d264f7f2b781 164 if(wait_flag == 1) {
Morimoto448 0:7e3b7c017977 165 // フラグの初期化
Morimoto448 0:7e3b7c017977 166 wait_flag = 0;
Morimoto448 0:7e3b7c017977 167 myled = 0; // LED消灯
Morimoto448 0:7e3b7c017977 168 // 左モータの制御
kato8 6:d264f7f2b781 169 mypwm4.pulsewidth_ms(50);
kato8 6:d264f7f2b781 170 mypwm6.pulsewidth_ms(50);
Morimoto448 0:7e3b7c017977 171 // 右モータの制御
kato8 6:d264f7f2b781 172 mypwm2.pulsewidth_ms(50);
kato8 6:d264f7f2b781 173 mypwm3.pulsewidth_ms(50);
Morimoto448 0:7e3b7c017977 174 // 500ms待機する
kato8 6:d264f7f2b781 175 mode = STATE_E;
Morimoto448 1:aabe6887447c 176 wait(0.5);
Morimoto448 0:7e3b7c017977 177 }
kato8 6:d264f7f2b781 178 */
kato8 6:d264f7f2b781 179 switch(mode) {
Morimoto448 0:7e3b7c017977 180 // STATE_A : 前進(左:正転 右:正転)
Morimoto448 0:7e3b7c017977 181 case STATE_A:
kato8 6:d264f7f2b781 182 printf("STATE_A\n");
Morimoto448 0:7e3b7c017977 183 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 184 // 左モータの制御
kato8 6:d264f7f2b781 185 mypwm4.pulsewidth_ms(15);
Morimoto448 1:aabe6887447c 186 mypwm6.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 187 // 右モータの制御
kato8 6:d264f7f2b781 188 mypwm2.pulsewidth_ms(15);
ishi777 2:f568ec2e6955 189 mypwm3.pulsewidth_ms(0);
ishi777 2:f568ec2e6955 190 break;
kato8 6:d264f7f2b781 191 // STATE_A2 : 右に曲がる(左:正転 右:正転)
ishi777 2:f568ec2e6955 192 case STATE_A2:
kato8 6:d264f7f2b781 193 printf("STATE_A2\n");
ishi777 2:f568ec2e6955 194 myled = 1; // LED点灯
ishi777 2:f568ec2e6955 195 // 左モータの制御
kato8 6:d264f7f2b781 196 mypwm4.pulsewidth_ms(10);
ishi777 2:f568ec2e6955 197 mypwm6.pulsewidth_ms(0);
ishi777 2:f568ec2e6955 198 // 右モータの制御
kato8 6:d264f7f2b781 199 mypwm2.pulsewidth_ms(6);
Morimoto448 4:04ca6a211b85 200 mypwm3.pulsewidth_ms(0);
Morimoto448 4:04ca6a211b85 201 break;
kato8 6:d264f7f2b781 202 // STATE_A3 : hidariに曲がる(左:正転 右:正転)
Morimoto448 4:04ca6a211b85 203 case STATE_A3:
kato8 6:d264f7f2b781 204 printf("STATE_A3\n");
Morimoto448 4:04ca6a211b85 205 myled = 1; // LED点灯
Morimoto448 4:04ca6a211b85 206 // 左モータの制御
kato8 6:d264f7f2b781 207 mypwm4.pulsewidth_ms(6);
Morimoto448 4:04ca6a211b85 208 mypwm6.pulsewidth_ms(0);
Morimoto448 4:04ca6a211b85 209 // 右モータの制御
Morimoto448 4:04ca6a211b85 210 mypwm2.pulsewidth_ms(10);
Morimoto448 0:7e3b7c017977 211 mypwm3.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 212 break;
Morimoto448 0:7e3b7c017977 213 // STATE_B : 右旋回(左:正転 右:逆転)
Morimoto448 0:7e3b7c017977 214 case STATE_B:
kato8 6:d264f7f2b781 215 printf("STATE_B\n");
Morimoto448 0:7e3b7c017977 216 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 217 // 左モータの制御
kato8 6:d264f7f2b781 218 mypwm4.pulsewidth_ms(15);
Morimoto448 1:aabe6887447c 219 mypwm6.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 220 // 右モータの制御
Morimoto448 0:7e3b7c017977 221 mypwm2.pulsewidth_ms(0);
kato8 6:d264f7f2b781 222 mypwm3.pulsewidth_ms(15);
Morimoto448 0:7e3b7c017977 223 break;
Morimoto448 0:7e3b7c017977 224 // STATE_C : 左旋回(左:逆転 右:正転)
Morimoto448 0:7e3b7c017977 225 case STATE_C:
kato8 6:d264f7f2b781 226 printf("STATE_C\n");
kato8 6:d264f7f2b781 227 myled = 1; // LED点灯
kato8 6:d264f7f2b781 228 // 左モータの制御
kato8 6:d264f7f2b781 229 mypwm4.pulsewidth_ms(0);
kato8 6:d264f7f2b781 230 mypwm6.pulsewidth_ms(15);
kato8 6:d264f7f2b781 231 // 右モータの制御
kato8 6:d264f7f2b781 232 mypwm2.pulsewidth_ms(15);
kato8 6:d264f7f2b781 233 mypwm3.pulsewidth_ms(0);
kato8 6:d264f7f2b781 234 break;
kato8 6:d264f7f2b781 235 // STATE_D : 後退(左:逆転 右:逆転)
kato8 6:d264f7f2b781 236 case STATE_D:
kato8 6:d264f7f2b781 237 printf("STATE_D\n");
Morimoto448 0:7e3b7c017977 238 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 239 // 左モータの制御
Morimoto448 0:7e3b7c017977 240 mypwm4.pulsewidth_ms(0);
kato8 6:d264f7f2b781 241 mypwm6.pulsewidth_ms(15);
Morimoto448 0:7e3b7c017977 242 // 右モータの制御
Morimoto448 0:7e3b7c017977 243 mypwm2.pulsewidth_ms(0);
kato8 6:d264f7f2b781 244 mypwm3.pulsewidth_ms(15);
Morimoto448 0:7e3b7c017977 245 break;
Morimoto448 0:7e3b7c017977 246 // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ)
Morimoto448 0:7e3b7c017977 247 case STATE_E:
kato8 6:d264f7f2b781 248 printf("STATE_E\n");
Morimoto448 0:7e3b7c017977 249 myled = 0; // LED消灯
Morimoto448 0:7e3b7c017977 250 // 左モータの制御
kato8 6:d264f7f2b781 251 mypwm4.pulsewidth_ms(30);
kato8 6:d264f7f2b781 252 mypwm6.pulsewidth_ms(30);
Morimoto448 0:7e3b7c017977 253 // 右モータの制御
kato8 6:d264f7f2b781 254 mypwm2.pulsewidth_ms(30);
kato8 6:d264f7f2b781 255 mypwm3.pulsewidth_ms(30);
kato8 6:d264f7f2b781 256 break;
kato8 6:d264f7f2b781 257 default:
Morimoto448 0:7e3b7c017977 258 break;
Morimoto448 0:7e3b7c017977 259 }
Morimoto448 0:7e3b7c017977 260 }
kato8 6:d264f7f2b781 261 }
kato8 6:d264f7f2b781 262
kato8 6:d264f7f2b781 263
kato8 6:d264f7f2b781 264 void LineTrace(void) { // ラインの右側でライントレース
kato8 6:d264f7f2b781 265 if(val_M > 0.5) { // 中央:白
kato8 6:d264f7f2b781 266 mode = STATE_A2; // 左前進
kato8 6:d264f7f2b781 267 } else { // 中央:黒
kato8 6:d264f7f2b781 268 mode = STATE_A3; // 右前進
kato8 6:d264f7f2b781 269 }
kato8 6:d264f7f2b781 270 wait_ms(50);
kato8 6:d264f7f2b781 271 }