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