Rui Kato / Mbed 2 deprecated Nucleo_SS2_No2

2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2 by SS2

main.cpp@5:ab74616c24b4, 2017-12-12 (annotated)

Committer:
kato8
Date:
Tue Dec 12 00:49:19 2017 +0000
Revision:
5:ab74616c24b4
Parent:
4:04ca6a211b85
Child:
6:d264f7f2b781
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
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
kato8 5:ab74616c24b4 60 while(1) {
Morimoto448 4:04ca6a211b85 61 usensor1.start();
Morimoto448 4:04ca6a211b85 62 usensor2.start();
kato8 5:ab74616c24b4 63 wait(0.2);
kato8 5:ab74616c24b4 64 dist1=usensor1.get_dist_cm();
kato8 5:ab74616c24b4 65 dist2=usensor2.get_dist_cm();
kato8 5:ab74616c24b4 66 /*printf("dist1: %ld\n",dist1); */
kato8 5:ab74616c24b4 67 printf("dist2: %ld\n",dist2);
Morimoto448 0:7e3b7c017977 68
Morimoto448 0:7e3b7c017977 69
Morimoto448 3:dd3b7c3c327b 70 /* switch(timer)
Morimoto448 1:aabe6887447c 71 {
Morimoto448 1:aabe6887447c 72 case 8.0:
Morimoto448 1:aabe6887447c 73 wait_flag = 1;
Morimoto448 1:aabe6887447c 74 mode = STATE_A;
Morimoto448 1:aabe6887447c 75 break;
Morimoto448 3:dd3b7c3c327b 76 case 12.0:
Morimoto448 1:aabe6887447c 77 wait_flag = 1;
Morimoto448 1:aabe6887447c 78 mode = STATE_B;
Morimoto448 1:aabe6887447c 79 break;
Morimoto448 3:dd3b7c3c327b 80 case 15.5:
Morimoto448 1:aabe6887447c 81 wait_flag = 1;
ishi777 2:f568ec2e6955 82 mode = STATE_A2;
Morimoto448 1:aabe6887447c 83 break;
ishi777 2:f568ec2e6955 84 case 22.0:
Morimoto448 1:aabe6887447c 85 wait_flag = 1;
Morimoto448 1:aabe6887447c 86 mode = STATE_D;
Morimoto448 1:aabe6887447c 87 break;
ishi777 2:f568ec2e6955 88 case 30.0:
Morimoto448 1:aabe6887447c 89 wait_flag = 1;
Morimoto448 1:aabe6887447c 90 mode = STATE_A;
Morimoto448 1:aabe6887447c 91 break;
ishi777 2:f568ec2e6955 92 case 37.0:
Morimoto448 1:aabe6887447c 93 wait_flag = 1;
Morimoto448 1:aabe6887447c 94 mode = STATE_C;
Morimoto448 1:aabe6887447c 95 break;
ishi777 2:f568ec2e6955 96 case 42.0:
Morimoto448 1:aabe6887447c 97 wait_flag = 1;
Morimoto448 1:aabe6887447c 98 mode = STATE_A;
Morimoto448 1:aabe6887447c 99 break;
Morimoto448 1:aabe6887447c 100
Morimoto448 3:dd3b7c3c327b 101 }*/
Morimoto448 3:dd3b7c3c327b 102
Morimoto448 3:dd3b7c3c327b 103 switch (fase)
Morimoto448 3:dd3b7c3c327b 104 {
Morimoto448 3:dd3b7c3c327b 105 case 0:
Morimoto448 4:04ca6a211b85 106 /*printf("timer: %d\n", timer);*/
Morimoto448 4:04ca6a211b85 107 if(timer > 5){
Morimoto448 4:04ca6a211b85 108 /*if(dist2 > 8){
Morimoto448 4:04ca6a211b85 109 mode = STATE_A3; //hidariに
Morimoto448 4:04ca6a211b85 110 }else if(dist2 < 10){
Morimoto448 4:04ca6a211b85 111 mode = STATE_A2; //migiに
Morimoto448 4:04ca6a211b85 112 }else{mode = STATE_A;}
Morimoto448 4:04ca6a211b85 113 }*/if(dist1 > 45 && dist1<250){
Morimoto448 4:04ca6a211b85 114 fase = 1;
Morimoto448 3:dd3b7c3c327b 115 t.stop();
Morimoto448 3:dd3b7c3c327b 116 t.reset();
Morimoto448 3:dd3b7c3c327b 117 t.start();
Morimoto448 3:dd3b7c3c327b 118 }
Morimoto448 4:04ca6a211b85 119 break;
Morimoto448 3:dd3b7c3c327b 120 case 1:
Morimoto448 3:dd3b7c3c327b 121 if(timer == 0){
Morimoto448 3:dd3b7c3c327b 122 wait_flag = 1;
Morimoto448 3:dd3b7c3c327b 123 mode = STATE_B;
Morimoto448 4:04ca6a211b85 124 }else if(timer > 2 && dist1 > 60){
Morimoto448 3:dd3b7c3c327b 125 wait_flag = 1;
Morimoto448 3:dd3b7c3c327b 126 mode = STATE_A;
Morimoto448 3:dd3b7c3c327b 127 t.reset();
Morimoto448 4:04ca6a211b85 128 t.stop();
Morimoto448 3:dd3b7c3c327b 129 fase = 2;
Morimoto448 3:dd3b7c3c327b 130 t.start();
Morimoto448 3:dd3b7c3c327b 131 }
Morimoto448 3:dd3b7c3c327b 132 break;
Morimoto448 3:dd3b7c3c327b 133 case 2:
Morimoto448 3:dd3b7c3c327b 134 if(timer == 6.0){
Morimoto448 3:dd3b7c3c327b 135 wait_flag = 1;
Morimoto448 3:dd3b7c3c327b 136 mode = STATE_D;
Morimoto448 3:dd3b7c3c327b 137 }else if(timer == 16.0){
Morimoto448 3:dd3b7c3c327b 138 wait_flag = 1;
Morimoto448 3:dd3b7c3c327b 139 mode = STATE_A;
Morimoto448 3:dd3b7c3c327b 140 }else if(timer == 26.0){
Morimoto448 3:dd3b7c3c327b 141 wait_flag = 1;
Morimoto448 3:dd3b7c3c327b 142 mode = STATE_D;
Morimoto448 3:dd3b7c3c327b 143 }else if(timer == 36.0){
Morimoto448 3:dd3b7c3c327b 144 wait_flag = 1;
Morimoto448 4:04ca6a211b85 145 mode = STATE_A;
Morimoto448 4:04ca6a211b85 146 }else if(timer == 46.0){
Morimoto448 3:dd3b7c3c327b 147 wait_flag = 1;
Morimoto448 4:04ca6a211b85 148 mode = STATE_D;}
Morimoto448 3:dd3b7c3c327b 149 }
Morimoto448 1:aabe6887447c 150
Morimoto448 1:aabe6887447c 151 // 状態が切り替わるときは一時停止
Morimoto448 0:7e3b7c017977 152 if(wait_flag==1)
Morimoto448 0:7e3b7c017977 153 {
Morimoto448 0:7e3b7c017977 154 // フラグの初期化
Morimoto448 0:7e3b7c017977 155 wait_flag = 0;
Morimoto448 0:7e3b7c017977 156 myled = 0; // LED消灯
Morimoto448 0:7e3b7c017977 157 // 左モータの制御
Morimoto448 0:7e3b7c017977 158 mypwm4.pulsewidth_ms(80);
Morimoto448 1:aabe6887447c 159 mypwm6.pulsewidth_ms(80);
Morimoto448 0:7e3b7c017977 160 // 右モータの制御
Morimoto448 0:7e3b7c017977 161 mypwm2.pulsewidth_ms(80);
Morimoto448 0:7e3b7c017977 162 mypwm3.pulsewidth_ms(80);
Morimoto448 0:7e3b7c017977 163 // 500ms待機する
Morimoto448 1:aabe6887447c 164 wait(0.5);
Morimoto448 0:7e3b7c017977 165 }
Morimoto448 3:dd3b7c3c327b 166
Morimoto448 0:7e3b7c017977 167 switch (mode)
Morimoto448 0:7e3b7c017977 168 {
Morimoto448 0:7e3b7c017977 169 // STATE_A : 前進（左：正転　右：正転）
Morimoto448 0:7e3b7c017977 170 case STATE_A:
Morimoto448 0:7e3b7c017977 171 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 172 // 左モータの制御
ishi777 2:f568ec2e6955 173 mypwm4.pulsewidth_ms(70);
Morimoto448 1:aabe6887447c 174 mypwm6.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 175 // 右モータの制御
Morimoto448 3:dd3b7c3c327b 176 mypwm2.pulsewidth_ms(70);
ishi777 2:f568ec2e6955 177 mypwm3.pulsewidth_ms(0);
ishi777 2:f568ec2e6955 178 break;
Morimoto448 4:04ca6a211b85 179 // STATE_A2 : 右に曲がる（左：正転　右：正転）
ishi777 2:f568ec2e6955 180 case STATE_A2:
ishi777 2:f568ec2e6955 181 myled = 1; // LED点灯
ishi777 2:f568ec2e6955 182 // 左モータの制御
Morimoto448 4:04ca6a211b85 183 mypwm4.pulsewidth_ms(50);
ishi777 2:f568ec2e6955 184 mypwm6.pulsewidth_ms(0);
ishi777 2:f568ec2e6955 185 // 右モータの制御
Morimoto448 4:04ca6a211b85 186 mypwm2.pulsewidth_ms(80);
Morimoto448 4:04ca6a211b85 187 mypwm3.pulsewidth_ms(0);
Morimoto448 4:04ca6a211b85 188 break;
Morimoto448 4:04ca6a211b85 189 // STATE_A3 : hidariに曲がる（左：正転　右：正転）
Morimoto448 4:04ca6a211b85 190 case STATE_A3:
Morimoto448 4:04ca6a211b85 191 myled = 1; // LED点灯
Morimoto448 4:04ca6a211b85 192 // 左モータの制御
Morimoto448 4:04ca6a211b85 193 mypwm4.pulsewidth_ms(80);
Morimoto448 4:04ca6a211b85 194 mypwm6.pulsewidth_ms(0);
Morimoto448 4:04ca6a211b85 195 // 右モータの制御
Morimoto448 4:04ca6a211b85 196 mypwm2.pulsewidth_ms(10);
Morimoto448 0:7e3b7c017977 197 mypwm3.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 198 break;
Morimoto448 0:7e3b7c017977 199
Morimoto448 0:7e3b7c017977 200 // STATE_B : 右旋回（左：正転　右：逆転）
Morimoto448 0:7e3b7c017977 201 case STATE_B:
Morimoto448 0:7e3b7c017977 202 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 203 // 左モータの制御
Morimoto448 3:dd3b7c3c327b 204 mypwm4.pulsewidth_ms(100);
Morimoto448 1:aabe6887447c 205 mypwm6.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 206 // 右モータの制御
Morimoto448 0:7e3b7c017977 207 mypwm2.pulsewidth_ms(0);
Morimoto448 3:dd3b7c3c327b 208 mypwm3.pulsewidth_ms(100);
Morimoto448 0:7e3b7c017977 209 break;
Morimoto448 0:7e3b7c017977 210
Morimoto448 0:7e3b7c017977 211 // STATE_C : 左旋回（左：逆転　右：正転）
Morimoto448 0:7e3b7c017977 212 case STATE_C:
Morimoto448 0:7e3b7c017977 213 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 214 // 左モータの制御
Morimoto448 0:7e3b7c017977 215 mypwm4.pulsewidth_ms(0);
Morimoto448 3:dd3b7c3c327b 216 mypwm6.pulsewidth_ms(100);
Morimoto448 0:7e3b7c017977 217 // 右モータの制御
Morimoto448 3:dd3b7c3c327b 218 mypwm2.pulsewidth_ms(100);
Morimoto448 0:7e3b7c017977 219 mypwm3.pulsewidth_ms(0);
Morimoto448 0:7e3b7c017977 220 break;
Morimoto448 0:7e3b7c017977 221
Morimoto448 0:7e3b7c017977 222 // STATE_D : 後退（左：逆転　右：逆転）
Morimoto448 0:7e3b7c017977 223 case STATE_D:
Morimoto448 0:7e3b7c017977 224 myled = 1; // LED点灯
Morimoto448 0:7e3b7c017977 225 // 左モータの制御
Morimoto448 0:7e3b7c017977 226 mypwm4.pulsewidth_ms(0);
ishi777 2:f568ec2e6955 227 mypwm6.pulsewidth_ms(80);
Morimoto448 0:7e3b7c017977 228 // 右モータの制御
Morimoto448 0:7e3b7c017977 229 mypwm2.pulsewidth_ms(0);
ishi777 2:f568ec2e6955 230 mypwm3.pulsewidth_ms(83);
Morimoto448 0:7e3b7c017977 231 break;
Morimoto448 0:7e3b7c017977 232
Morimoto448 0:7e3b7c017977 233 // STATE_E : ブレーキ（左：ブレーキ　右：ブレーキ）
Morimoto448 0:7e3b7c017977 234 case STATE_E:
Morimoto448 0:7e3b7c017977 235 myled = 0; // LED消灯
Morimoto448 0:7e3b7c017977 236 // 左モータの制御
Morimoto448 0:7e3b7c017977 237 mypwm4.pulsewidth_ms(60);
Morimoto448 1:aabe6887447c 238 mypwm6.pulsewidth_ms(60);
Morimoto448 0:7e3b7c017977 239 // 右モータの制御
Morimoto448 0:7e3b7c017977 240 mypwm2.pulsewidth_ms(60);
Morimoto448 0:7e3b7c017977 241 mypwm3.pulsewidth_ms(60);
Morimoto448 0:7e3b7c017977 242 break;
Morimoto448 0:7e3b7c017977 243 }
Morimoto448 0:7e3b7c017977 244 }
kato8 5:ab74616c24b4 245 } }