Rui Kato / Mbed 2 deprecated Nucleo_SS2_No2

2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2 by SS2

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?

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
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 }