Rui Kato
/
Nucleo_SS2_No2
2号機
Fork of Nucleo_SS2 by
main.cpp@6:d264f7f2b781, 2017-12-19 (annotated)
- Committer:
- kato8
- Date:
- Tue Dec 19 01:23:44 2017 +0000
- Revision:
- 6:d264f7f2b781
- Parent:
- 5:ab74616c24b4
2??
Who changed what in which revision?
User | Revision | Line number | New 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 | } |