Rui Kato
/
Nucleo_SS2_No2
2号機
Fork of Nucleo_SS2 by
main.cpp@4:04ca6a211b85, 2017-12-12 (annotated)
- Committer:
- Morimoto448
- Date:
- Tue Dec 12 00:34:36 2017 +0000
- Revision:
- 4:04ca6a211b85
- Parent:
- 3:dd3b7c3c327b
- Child:
- 5:ab74616c24b4
test
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 | |
Morimoto448 | 0:7e3b7c017977 | 4 | PwmOut mypwm4(D4); |
Morimoto448 | 1:aabe6887447c | 5 | PwmOut mypwm6(D6); |
Morimoto448 | 0:7e3b7c017977 | 6 | PwmOut mypwm2(D2); |
Morimoto448 | 0:7e3b7c017977 | 7 | PwmOut mypwm3(D3); |
Morimoto448 | 0:7e3b7c017977 | 8 | |
Morimoto448 | 0:7e3b7c017977 | 9 | Ticker flag; |
Morimoto448 | 3:dd3b7c3c327b | 10 | Ticker timerflag; |
Morimoto448 | 1:aabe6887447c | 11 | Timer t; |
Morimoto448 | 0:7e3b7c017977 | 12 | |
Morimoto448 | 0:7e3b7c017977 | 13 | DigitalOut myled(LED1); |
Morimoto448 | 0:7e3b7c017977 | 14 | |
Morimoto448 | 4:04ca6a211b85 | 15 | HCSR04 usensor1(D9,D10); // Trigger(DO), Echo(PWMIN) |
Morimoto448 | 4:04ca6a211b85 | 16 | HCSR04 usensor2(D11,D12); // Trigger(DO), Echo(PWMIN) |
Morimoto448 | 4:04ca6a211b85 | 17 | unsigned int dist1,dist2; |
Morimoto448 | 3:dd3b7c3c327b | 18 | |
Morimoto448 | 0:7e3b7c017977 | 19 | |
Morimoto448 | 0:7e3b7c017977 | 20 | #define STATE_A (0) |
Morimoto448 | 0:7e3b7c017977 | 21 | #define STATE_B (1) |
Morimoto448 | 0:7e3b7c017977 | 22 | #define STATE_C (2) |
Morimoto448 | 0:7e3b7c017977 | 23 | #define STATE_D (3) |
Morimoto448 | 0:7e3b7c017977 | 24 | #define STATE_E (4) |
ishi777 | 2:f568ec2e6955 | 25 | #define STATE_A2 (5) |
Morimoto448 | 4:04ca6a211b85 | 26 | #define STATE_A3 (6) |
Morimoto448 | 4:04ca6a211b85 | 27 | |
Morimoto448 | 0:7e3b7c017977 | 28 | |
Morimoto448 | 3:dd3b7c3c327b | 29 | char mode,fase; |
Morimoto448 | 0:7e3b7c017977 | 30 | int wait_flag; |
Morimoto448 | 1:aabe6887447c | 31 | int timer; |
Morimoto448 | 0:7e3b7c017977 | 32 | |
Morimoto448 | 4:04ca6a211b85 | 33 | |
Morimoto448 | 3:dd3b7c3c327b | 34 | void tflg(){ |
Morimoto448 | 1:aabe6887447c | 35 | timer = t.read(); |
Morimoto448 | 0:7e3b7c017977 | 36 | } |
Morimoto448 | 3:dd3b7c3c327b | 37 | /*void flg(){ |
Morimoto448 | 3:dd3b7c3c327b | 38 | usensor.start(); |
Morimoto448 | 3:dd3b7c3c327b | 39 | dist=usensor.get_dist_cm(); |
Morimoto448 | 3:dd3b7c3c327b | 40 | printf("dist:%u\n", dist); |
Morimoto448 | 3:dd3b7c3c327b | 41 | }*/ |
Morimoto448 | 0:7e3b7c017977 | 42 | |
Morimoto448 | 0:7e3b7c017977 | 43 | int main() { |
Morimoto448 | 0:7e3b7c017977 | 44 | |
Morimoto448 | 1:aabe6887447c | 45 | t.start(); |
Morimoto448 | 0:7e3b7c017977 | 46 | mode = STATE_E; |
Morimoto448 | 0:7e3b7c017977 | 47 | wait_flag = 0; |
Morimoto448 | 0:7e3b7c017977 | 48 | |
Morimoto448 | 4:04ca6a211b85 | 49 | fase = 0; |
Morimoto448 | 4:04ca6a211b85 | 50 | |
Morimoto448 | 0:7e3b7c017977 | 51 | mypwm4.period_ms(100); |
Morimoto448 | 1:aabe6887447c | 52 | mypwm6.period_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 53 | mypwm2.period_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 54 | mypwm3.period_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 55 | |
Morimoto448 | 4:04ca6a211b85 | 56 | timerflag.attach(&tflg, 1.0); |
Morimoto448 | 3:dd3b7c3c327b | 57 | /*flag.attach(&flg, 1.0);*/ |
Morimoto448 | 3:dd3b7c3c327b | 58 | |
Morimoto448 | 3:dd3b7c3c327b | 59 | |
Morimoto448 | 3:dd3b7c3c327b | 60 | while(1){ |
Morimoto448 | 3:dd3b7c3c327b | 61 | |
Morimoto448 | 4:04ca6a211b85 | 62 | usensor1.start(); |
Morimoto448 | 4:04ca6a211b85 | 63 | usensor2.start(); |
Morimoto448 | 3:dd3b7c3c327b | 64 | wait(0.2); |
Morimoto448 | 4:04ca6a211b85 | 65 | dist1=usensor1.get_dist_cm(); |
Morimoto448 | 4:04ca6a211b85 | 66 | dist2=usensor2.get_dist_cm(); |
Morimoto448 | 4:04ca6a211b85 | 67 | /*printf("dist1: %ld\n",dist1); */ |
Morimoto448 | 4:04ca6a211b85 | 68 | printf("dist2: %ld\n",dist2); |
Morimoto448 | 0:7e3b7c017977 | 69 | |
Morimoto448 | 0:7e3b7c017977 | 70 | |
Morimoto448 | 3:dd3b7c3c327b | 71 | /* switch(timer) |
Morimoto448 | 1:aabe6887447c | 72 | { |
Morimoto448 | 1:aabe6887447c | 73 | case 8.0: |
Morimoto448 | 1:aabe6887447c | 74 | wait_flag = 1; |
Morimoto448 | 1:aabe6887447c | 75 | mode = STATE_A; |
Morimoto448 | 1:aabe6887447c | 76 | break; |
Morimoto448 | 3:dd3b7c3c327b | 77 | case 12.0: |
Morimoto448 | 1:aabe6887447c | 78 | wait_flag = 1; |
Morimoto448 | 1:aabe6887447c | 79 | mode = STATE_B; |
Morimoto448 | 1:aabe6887447c | 80 | break; |
Morimoto448 | 3:dd3b7c3c327b | 81 | case 15.5: |
Morimoto448 | 1:aabe6887447c | 82 | wait_flag = 1; |
ishi777 | 2:f568ec2e6955 | 83 | mode = STATE_A2; |
Morimoto448 | 1:aabe6887447c | 84 | break; |
ishi777 | 2:f568ec2e6955 | 85 | case 22.0: |
Morimoto448 | 1:aabe6887447c | 86 | wait_flag = 1; |
Morimoto448 | 1:aabe6887447c | 87 | mode = STATE_D; |
Morimoto448 | 1:aabe6887447c | 88 | break; |
ishi777 | 2:f568ec2e6955 | 89 | case 30.0: |
Morimoto448 | 1:aabe6887447c | 90 | wait_flag = 1; |
Morimoto448 | 1:aabe6887447c | 91 | mode = STATE_A; |
Morimoto448 | 1:aabe6887447c | 92 | break; |
ishi777 | 2:f568ec2e6955 | 93 | case 37.0: |
Morimoto448 | 1:aabe6887447c | 94 | wait_flag = 1; |
Morimoto448 | 1:aabe6887447c | 95 | mode = STATE_C; |
Morimoto448 | 1:aabe6887447c | 96 | break; |
ishi777 | 2:f568ec2e6955 | 97 | case 42.0: |
Morimoto448 | 1:aabe6887447c | 98 | wait_flag = 1; |
Morimoto448 | 1:aabe6887447c | 99 | mode = STATE_A; |
Morimoto448 | 1:aabe6887447c | 100 | break; |
Morimoto448 | 1:aabe6887447c | 101 | |
Morimoto448 | 3:dd3b7c3c327b | 102 | }*/ |
Morimoto448 | 3:dd3b7c3c327b | 103 | |
Morimoto448 | 3:dd3b7c3c327b | 104 | switch (fase) |
Morimoto448 | 3:dd3b7c3c327b | 105 | { |
Morimoto448 | 3:dd3b7c3c327b | 106 | case 0: |
Morimoto448 | 4:04ca6a211b85 | 107 | /*printf("timer: %d\n", timer);*/ |
Morimoto448 | 4:04ca6a211b85 | 108 | if(timer > 5){ |
Morimoto448 | 4:04ca6a211b85 | 109 | /*if(dist2 > 8){ |
Morimoto448 | 4:04ca6a211b85 | 110 | mode = STATE_A3; //hidariに |
Morimoto448 | 4:04ca6a211b85 | 111 | }else if(dist2 < 10){ |
Morimoto448 | 4:04ca6a211b85 | 112 | mode = STATE_A2; //migiに |
Morimoto448 | 4:04ca6a211b85 | 113 | }else{mode = STATE_A;} |
Morimoto448 | 4:04ca6a211b85 | 114 | }*/if(dist1 > 45 && dist1<250){ |
Morimoto448 | 4:04ca6a211b85 | 115 | fase = 1; |
Morimoto448 | 3:dd3b7c3c327b | 116 | t.stop(); |
Morimoto448 | 3:dd3b7c3c327b | 117 | t.reset(); |
Morimoto448 | 3:dd3b7c3c327b | 118 | t.start(); |
Morimoto448 | 3:dd3b7c3c327b | 119 | } |
Morimoto448 | 4:04ca6a211b85 | 120 | break; |
Morimoto448 | 3:dd3b7c3c327b | 121 | case 1: |
Morimoto448 | 3:dd3b7c3c327b | 122 | if(timer == 0){ |
Morimoto448 | 3:dd3b7c3c327b | 123 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 124 | mode = STATE_B; |
Morimoto448 | 4:04ca6a211b85 | 125 | }else if(timer > 2 && dist1 > 60){ |
Morimoto448 | 3:dd3b7c3c327b | 126 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 127 | mode = STATE_A; |
Morimoto448 | 3:dd3b7c3c327b | 128 | t.reset(); |
Morimoto448 | 4:04ca6a211b85 | 129 | t.stop(); |
Morimoto448 | 3:dd3b7c3c327b | 130 | fase = 2; |
Morimoto448 | 3:dd3b7c3c327b | 131 | t.start(); |
Morimoto448 | 3:dd3b7c3c327b | 132 | } |
Morimoto448 | 3:dd3b7c3c327b | 133 | break; |
Morimoto448 | 3:dd3b7c3c327b | 134 | case 2: |
Morimoto448 | 3:dd3b7c3c327b | 135 | if(timer == 6.0){ |
Morimoto448 | 3:dd3b7c3c327b | 136 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 137 | mode = STATE_D; |
Morimoto448 | 3:dd3b7c3c327b | 138 | }else if(timer == 16.0){ |
Morimoto448 | 3:dd3b7c3c327b | 139 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 140 | mode = STATE_A; |
Morimoto448 | 3:dd3b7c3c327b | 141 | }else if(timer == 26.0){ |
Morimoto448 | 3:dd3b7c3c327b | 142 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 143 | mode = STATE_D; |
Morimoto448 | 3:dd3b7c3c327b | 144 | }else if(timer == 36.0){ |
Morimoto448 | 3:dd3b7c3c327b | 145 | wait_flag = 1; |
Morimoto448 | 4:04ca6a211b85 | 146 | mode = STATE_A; |
Morimoto448 | 4:04ca6a211b85 | 147 | }else if(timer == 46.0){ |
Morimoto448 | 3:dd3b7c3c327b | 148 | wait_flag = 1; |
Morimoto448 | 4:04ca6a211b85 | 149 | mode = STATE_D;} |
Morimoto448 | 3:dd3b7c3c327b | 150 | } |
Morimoto448 | 1:aabe6887447c | 151 | |
Morimoto448 | 1:aabe6887447c | 152 | // 状態が切り替わるときは一時停止 |
Morimoto448 | 0:7e3b7c017977 | 153 | if(wait_flag==1) |
Morimoto448 | 0:7e3b7c017977 | 154 | { |
Morimoto448 | 0:7e3b7c017977 | 155 | // フラグの初期化 |
Morimoto448 | 0:7e3b7c017977 | 156 | wait_flag = 0; |
Morimoto448 | 0:7e3b7c017977 | 157 | myled = 0; // LED消灯 |
Morimoto448 | 0:7e3b7c017977 | 158 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 159 | mypwm4.pulsewidth_ms(80); |
Morimoto448 | 1:aabe6887447c | 160 | mypwm6.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 161 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 162 | mypwm2.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 163 | mypwm3.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 164 | // 500ms待機する |
Morimoto448 | 1:aabe6887447c | 165 | wait(0.5); |
Morimoto448 | 0:7e3b7c017977 | 166 | } |
Morimoto448 | 3:dd3b7c3c327b | 167 | |
Morimoto448 | 0:7e3b7c017977 | 168 | switch (mode) |
Morimoto448 | 0:7e3b7c017977 | 169 | { |
Morimoto448 | 0:7e3b7c017977 | 170 | // STATE_A : 前進(左:正転 右:正転) |
Morimoto448 | 0:7e3b7c017977 | 171 | case STATE_A: |
Morimoto448 | 0:7e3b7c017977 | 172 | myled = 1; // LED点灯 |
Morimoto448 | 0:7e3b7c017977 | 173 | // 左モータの制御 |
ishi777 | 2:f568ec2e6955 | 174 | mypwm4.pulsewidth_ms(70); |
Morimoto448 | 1:aabe6887447c | 175 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 0:7e3b7c017977 | 176 | // 右モータの制御 |
Morimoto448 | 3:dd3b7c3c327b | 177 | mypwm2.pulsewidth_ms(70); |
ishi777 | 2:f568ec2e6955 | 178 | mypwm3.pulsewidth_ms(0); |
ishi777 | 2:f568ec2e6955 | 179 | break; |
Morimoto448 | 4:04ca6a211b85 | 180 | // STATE_A2 : 右に曲がる(左:正転 右:正転) |
ishi777 | 2:f568ec2e6955 | 181 | case STATE_A2: |
ishi777 | 2:f568ec2e6955 | 182 | myled = 1; // LED点灯 |
ishi777 | 2:f568ec2e6955 | 183 | // 左モータの制御 |
Morimoto448 | 4:04ca6a211b85 | 184 | mypwm4.pulsewidth_ms(50); |
ishi777 | 2:f568ec2e6955 | 185 | mypwm6.pulsewidth_ms(0); |
ishi777 | 2:f568ec2e6955 | 186 | // 右モータの制御 |
Morimoto448 | 4:04ca6a211b85 | 187 | mypwm2.pulsewidth_ms(80); |
Morimoto448 | 4:04ca6a211b85 | 188 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 4:04ca6a211b85 | 189 | break; |
Morimoto448 | 4:04ca6a211b85 | 190 | // STATE_A3 : hidariに曲がる(左:正転 右:正転) |
Morimoto448 | 4:04ca6a211b85 | 191 | case STATE_A3: |
Morimoto448 | 4:04ca6a211b85 | 192 | myled = 1; // LED点灯 |
Morimoto448 | 4:04ca6a211b85 | 193 | // 左モータの制御 |
Morimoto448 | 4:04ca6a211b85 | 194 | mypwm4.pulsewidth_ms(80); |
Morimoto448 | 4:04ca6a211b85 | 195 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 4:04ca6a211b85 | 196 | // 右モータの制御 |
Morimoto448 | 4:04ca6a211b85 | 197 | mypwm2.pulsewidth_ms(10); |
Morimoto448 | 0:7e3b7c017977 | 198 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 0:7e3b7c017977 | 199 | break; |
Morimoto448 | 0:7e3b7c017977 | 200 | |
Morimoto448 | 0:7e3b7c017977 | 201 | // STATE_B : 右旋回(左:正転 右:逆転) |
Morimoto448 | 0:7e3b7c017977 | 202 | case STATE_B: |
Morimoto448 | 0:7e3b7c017977 | 203 | myled = 1; // LED点灯 |
Morimoto448 | 0:7e3b7c017977 | 204 | // 左モータの制御 |
Morimoto448 | 3:dd3b7c3c327b | 205 | mypwm4.pulsewidth_ms(100); |
Morimoto448 | 1:aabe6887447c | 206 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 0:7e3b7c017977 | 207 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 208 | mypwm2.pulsewidth_ms(0); |
Morimoto448 | 3:dd3b7c3c327b | 209 | mypwm3.pulsewidth_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 210 | break; |
Morimoto448 | 0:7e3b7c017977 | 211 | |
Morimoto448 | 0:7e3b7c017977 | 212 | // STATE_C : 左旋回(左:逆転 右:正転) |
Morimoto448 | 0:7e3b7c017977 | 213 | case STATE_C: |
Morimoto448 | 0:7e3b7c017977 | 214 | myled = 1; // LED点灯 |
Morimoto448 | 0:7e3b7c017977 | 215 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 216 | mypwm4.pulsewidth_ms(0); |
Morimoto448 | 3:dd3b7c3c327b | 217 | mypwm6.pulsewidth_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 218 | // 右モータの制御 |
Morimoto448 | 3:dd3b7c3c327b | 219 | mypwm2.pulsewidth_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 220 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 0:7e3b7c017977 | 221 | break; |
Morimoto448 | 0:7e3b7c017977 | 222 | |
Morimoto448 | 0:7e3b7c017977 | 223 | // STATE_D : 後退(左:逆転 右:逆転) |
Morimoto448 | 0:7e3b7c017977 | 224 | case STATE_D: |
Morimoto448 | 0:7e3b7c017977 | 225 | myled = 1; // LED点灯 |
Morimoto448 | 0:7e3b7c017977 | 226 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 227 | mypwm4.pulsewidth_ms(0); |
ishi777 | 2:f568ec2e6955 | 228 | mypwm6.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 229 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 230 | mypwm2.pulsewidth_ms(0); |
ishi777 | 2:f568ec2e6955 | 231 | mypwm3.pulsewidth_ms(83); |
Morimoto448 | 0:7e3b7c017977 | 232 | break; |
Morimoto448 | 0:7e3b7c017977 | 233 | |
Morimoto448 | 0:7e3b7c017977 | 234 | // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ) |
Morimoto448 | 0:7e3b7c017977 | 235 | case STATE_E: |
Morimoto448 | 0:7e3b7c017977 | 236 | myled = 0; // LED消灯 |
Morimoto448 | 0:7e3b7c017977 | 237 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 238 | mypwm4.pulsewidth_ms(60); |
Morimoto448 | 1:aabe6887447c | 239 | mypwm6.pulsewidth_ms(60); |
Morimoto448 | 0:7e3b7c017977 | 240 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 241 | mypwm2.pulsewidth_ms(60); |
Morimoto448 | 0:7e3b7c017977 | 242 | mypwm3.pulsewidth_ms(60); |
Morimoto448 | 0:7e3b7c017977 | 243 | break; |
Morimoto448 | 0:7e3b7c017977 | 244 | } |
Morimoto448 | 0:7e3b7c017977 | 245 | } |
Morimoto448 | 4:04ca6a211b85 | 246 | } |