Rui Kato / Mbed 2 deprecated Nucleo_SS2_No2

2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2 by SS2

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?

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