SS2
/
Nucleo_SS2
test
main.cpp@5:b6daf80a7237, 2018-01-15 (annotated)
- Committer:
- Morimoto448
- Date:
- Mon Jan 15 07:02:03 2018 +0000
- Revision:
- 5:b6daf80a7237
- Parent:
- 4:04ca6a211b85
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 | 5:b6daf80a7237 | 4 | |
Morimoto448 | 5:b6daf80a7237 | 5 | AnalogIn photoref_L(A1); // フォトリフレクタ左 |
Morimoto448 | 5:b6daf80a7237 | 6 | AnalogIn photoref_M(A2); // フォトリフレクタ中央 |
Morimoto448 | 5:b6daf80a7237 | 7 | AnalogIn photoref_R(A3); // フォトリフレクタ右 |
Morimoto448 | 5:b6daf80a7237 | 8 | |
Morimoto448 | 0:7e3b7c017977 | 9 | PwmOut mypwm4(D4); |
Morimoto448 | 1:aabe6887447c | 10 | PwmOut mypwm6(D6); |
Morimoto448 | 0:7e3b7c017977 | 11 | PwmOut mypwm2(D2); |
Morimoto448 | 0:7e3b7c017977 | 12 | PwmOut mypwm3(D3); |
Morimoto448 | 0:7e3b7c017977 | 13 | |
Morimoto448 | 0:7e3b7c017977 | 14 | Ticker flag; |
Morimoto448 | 3:dd3b7c3c327b | 15 | Ticker timerflag; |
Morimoto448 | 1:aabe6887447c | 16 | Timer t; |
Morimoto448 | 0:7e3b7c017977 | 17 | |
Morimoto448 | 0:7e3b7c017977 | 18 | DigitalOut myled(LED1); |
Morimoto448 | 0:7e3b7c017977 | 19 | |
Morimoto448 | 4:04ca6a211b85 | 20 | HCSR04 usensor1(D9,D10); // Trigger(DO), Echo(PWMIN) |
Morimoto448 | 4:04ca6a211b85 | 21 | HCSR04 usensor2(D11,D12); // Trigger(DO), Echo(PWMIN) |
Morimoto448 | 4:04ca6a211b85 | 22 | unsigned int dist1,dist2; |
Morimoto448 | 3:dd3b7c3c327b | 23 | |
Morimoto448 | 0:7e3b7c017977 | 24 | |
Morimoto448 | 0:7e3b7c017977 | 25 | #define STATE_A (0) |
Morimoto448 | 0:7e3b7c017977 | 26 | #define STATE_B (1) |
Morimoto448 | 0:7e3b7c017977 | 27 | #define STATE_C (2) |
Morimoto448 | 0:7e3b7c017977 | 28 | #define STATE_D (3) |
Morimoto448 | 0:7e3b7c017977 | 29 | #define STATE_E (4) |
ishi777 | 2:f568ec2e6955 | 30 | #define STATE_A2 (5) |
Morimoto448 | 4:04ca6a211b85 | 31 | #define STATE_A3 (6) |
Morimoto448 | 5:b6daf80a7237 | 32 | #define STATE_A4 (7) |
Morimoto448 | 5:b6daf80a7237 | 33 | #define STATE_D2 (8) |
Morimoto448 | 0:7e3b7c017977 | 34 | |
Morimoto448 | 5:b6daf80a7237 | 35 | char mode,fase,linetrace; |
Morimoto448 | 0:7e3b7c017977 | 36 | int wait_flag; |
Morimoto448 | 5:b6daf80a7237 | 37 | double timer,measL,measM,measR; |
Morimoto448 | 0:7e3b7c017977 | 38 | |
Morimoto448 | 4:04ca6a211b85 | 39 | |
Morimoto448 | 3:dd3b7c3c327b | 40 | void tflg(){ |
Morimoto448 | 5:b6daf80a7237 | 41 | timer = t.read(); //タイマー値読み込み |
Morimoto448 | 0:7e3b7c017977 | 42 | } |
Morimoto448 | 3:dd3b7c3c327b | 43 | /*void flg(){ |
Morimoto448 | 3:dd3b7c3c327b | 44 | usensor.start(); |
Morimoto448 | 3:dd3b7c3c327b | 45 | dist=usensor.get_dist_cm(); |
Morimoto448 | 3:dd3b7c3c327b | 46 | printf("dist:%u\n", dist); |
Morimoto448 | 3:dd3b7c3c327b | 47 | }*/ |
Morimoto448 | 0:7e3b7c017977 | 48 | |
Morimoto448 | 0:7e3b7c017977 | 49 | int main() { |
Morimoto448 | 0:7e3b7c017977 | 50 | |
Morimoto448 | 1:aabe6887447c | 51 | t.start(); |
Morimoto448 | 0:7e3b7c017977 | 52 | mode = STATE_E; |
Morimoto448 | 0:7e3b7c017977 | 53 | wait_flag = 0; |
Morimoto448 | 0:7e3b7c017977 | 54 | |
Morimoto448 | 4:04ca6a211b85 | 55 | fase = 0; |
Morimoto448 | 4:04ca6a211b85 | 56 | |
Morimoto448 | 0:7e3b7c017977 | 57 | mypwm4.period_ms(100); |
Morimoto448 | 1:aabe6887447c | 58 | mypwm6.period_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 59 | mypwm2.period_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 60 | mypwm3.period_ms(100); |
Morimoto448 | 0:7e3b7c017977 | 61 | |
Morimoto448 | 5:b6daf80a7237 | 62 | timerflag.attach(&tflg, 0.1); |
Morimoto448 | 3:dd3b7c3c327b | 63 | /*flag.attach(&flg, 1.0);*/ |
Morimoto448 | 3:dd3b7c3c327b | 64 | |
Morimoto448 | 3:dd3b7c3c327b | 65 | |
Morimoto448 | 3:dd3b7c3c327b | 66 | while(1){ |
Morimoto448 | 3:dd3b7c3c327b | 67 | |
Morimoto448 | 4:04ca6a211b85 | 68 | usensor1.start(); |
Morimoto448 | 5:b6daf80a7237 | 69 | usensor2.start(); |
Morimoto448 | 4:04ca6a211b85 | 70 | dist1=usensor1.get_dist_cm(); |
Morimoto448 | 4:04ca6a211b85 | 71 | dist2=usensor2.get_dist_cm(); |
Morimoto448 | 5:b6daf80a7237 | 72 | measL = photoref_L.read(); |
Morimoto448 | 5:b6daf80a7237 | 73 | measM = photoref_M.read(); |
Morimoto448 | 5:b6daf80a7237 | 74 | measR = photoref_R.read(); |
Morimoto448 | 5:b6daf80a7237 | 75 | wait(0.1); |
Morimoto448 | 5:b6daf80a7237 | 76 | printf("dist1: %ld\n",dist1); |
Morimoto448 | 4:04ca6a211b85 | 77 | printf("dist2: %ld\n",dist2); |
Morimoto448 | 5:b6daf80a7237 | 78 | printf("fase: %ld\n",fase); |
Morimoto448 | 5:b6daf80a7237 | 79 | printf("timer: %lf\n",timer); |
Morimoto448 | 0:7e3b7c017977 | 80 | |
Morimoto448 | 5:b6daf80a7237 | 81 | |
Morimoto448 | 5:b6daf80a7237 | 82 | |
Morimoto448 | 3:dd3b7c3c327b | 83 | |
Morimoto448 | 3:dd3b7c3c327b | 84 | switch (fase) |
Morimoto448 | 5:b6daf80a7237 | 85 | { case 0: |
Morimoto448 | 5:b6daf80a7237 | 86 | if(timer > 5.0){ |
Morimoto448 | 5:b6daf80a7237 | 87 | mode = STATE_A2; |
Morimoto448 | 5:b6daf80a7237 | 88 | if(dist1 > 45 && dist1 < 100){ |
Morimoto448 | 5:b6daf80a7237 | 89 | fase = 1; |
Morimoto448 | 5:b6daf80a7237 | 90 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 91 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 92 | } |
Morimoto448 | 5:b6daf80a7237 | 93 | } |
Morimoto448 | 5:b6daf80a7237 | 94 | break; |
Morimoto448 | 5:b6daf80a7237 | 95 | /* switch(linetrace){ |
Morimoto448 | 5:b6daf80a7237 | 96 | case 0: |
Morimoto448 | 5:b6daf80a7237 | 97 | if(timer > 5 && measL>0.2 && measM>0.2 && measR>0.2){ |
Morimoto448 | 5:b6daf80a7237 | 98 | mode = STATE_A4; |
Morimoto448 | 5:b6daf80a7237 | 99 | }if((timer > 5 && measL<0.2 && measM<0.2) || (timer > 5 && measM<0.2 && measR<0.2)){ |
Morimoto448 | 5:b6daf80a7237 | 100 | linetrace = 1; |
Morimoto448 | 5:b6daf80a7237 | 101 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 102 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 103 | } |
Morimoto448 | 5:b6daf80a7237 | 104 | break; |
Morimoto448 | 5:b6daf80a7237 | 105 | |
Morimoto448 | 5:b6daf80a7237 | 106 | case 1: |
Morimoto448 | 5:b6daf80a7237 | 107 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 108 | if(measL<0.2 && measM>0.2){ |
Morimoto448 | 5:b6daf80a7237 | 109 | mode = STATE_A3; |
Morimoto448 | 5:b6daf80a7237 | 110 | }else if(measM<0.2 && measL>0.2){ |
Morimoto448 | 5:b6daf80a7237 | 111 | mode = STATE_A2; |
Morimoto448 | 5:b6daf80a7237 | 112 | }else if(measL>0.2 && measM>0.2){ |
Morimoto448 | 5:b6daf80a7237 | 113 | if(dist2 < 10){ |
Morimoto448 | 5:b6daf80a7237 | 114 | mode = STATE_A2; |
Morimoto448 | 5:b6daf80a7237 | 115 | }else if(dist2 > 10){ |
Morimoto448 | 5:b6daf80a7237 | 116 | mode = STATE_A3; |
Morimoto448 | 5:b6daf80a7237 | 117 | } |
Morimoto448 | 5:b6daf80a7237 | 118 | }else if((timer > 1 && measL<0.2 && measM<0.2) || (timer > 1 && measM<0.2 && measR<0.2) ){ |
Morimoto448 | 5:b6daf80a7237 | 119 | linetrace = 2; |
Morimoto448 | 5:b6daf80a7237 | 120 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 121 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 122 | } |
Morimoto448 | 5:b6daf80a7237 | 123 | break; |
Morimoto448 | 5:b6daf80a7237 | 124 | |
Morimoto448 | 5:b6daf80a7237 | 125 | case 2: |
Morimoto448 | 5:b6daf80a7237 | 126 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 127 | mode = STATE_B; |
Morimoto448 | 5:b6daf80a7237 | 128 | if((timer > 2 && measL<0.2 && measM<0.2) || (timer > 1 && measM<0.2 && measR<0.2)){ |
Morimoto448 | 5:b6daf80a7237 | 129 | fase = 2; |
Morimoto448 | 3:dd3b7c3c327b | 130 | t.stop(); |
Morimoto448 | 3:dd3b7c3c327b | 131 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 132 | } //if閉じ |
Morimoto448 | 5:b6daf80a7237 | 133 | } //switch閉じ |
Morimoto448 | 5:b6daf80a7237 | 134 | break;*/ |
Morimoto448 | 5:b6daf80a7237 | 135 | |
Morimoto448 | 3:dd3b7c3c327b | 136 | case 1: |
Morimoto448 | 5:b6daf80a7237 | 137 | if(timer == 0){ |
Morimoto448 | 5:b6daf80a7237 | 138 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 139 | mode = STATE_B; |
Morimoto448 | 5:b6daf80a7237 | 140 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 141 | }else if(timer > 2.0 && dist1 > 60 && dist1 < 250){ |
Morimoto448 | 3:dd3b7c3c327b | 142 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 143 | mode = STATE_A; |
Morimoto448 | 5:b6daf80a7237 | 144 | t.stop(); |
Morimoto448 | 3:dd3b7c3c327b | 145 | t.reset(); |
Morimoto448 | 3:dd3b7c3c327b | 146 | fase = 2; |
Morimoto448 | 3:dd3b7c3c327b | 147 | } |
Morimoto448 | 3:dd3b7c3c327b | 148 | break; |
Morimoto448 | 3:dd3b7c3c327b | 149 | case 2: |
Morimoto448 | 5:b6daf80a7237 | 150 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 151 | if(timer > 10.0 && timer < 11.0){ |
Morimoto448 | 3:dd3b7c3c327b | 152 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 153 | mode = STATE_D; |
Morimoto448 | 5:b6daf80a7237 | 154 | fase = 3; |
Morimoto448 | 5:b6daf80a7237 | 155 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 156 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 157 | }/*else if (timer > 10.0 && timer < 11.0 && dist2 < 8 ){ |
Morimoto448 | 5:b6daf80a7237 | 158 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 159 | mode = STATE_D2; |
Morimoto448 | 5:b6daf80a7237 | 160 | fase = 3; |
Morimoto448 | 5:b6daf80a7237 | 161 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 162 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 163 | }*/ |
Morimoto448 | 5:b6daf80a7237 | 164 | break; |
Morimoto448 | 5:b6daf80a7237 | 165 | case 3: |
Morimoto448 | 5:b6daf80a7237 | 166 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 167 | /* if(timer > 15.0 && timer < 16.0 && dist2 > 8 && dist2 < 100){ |
Morimoto448 | 5:b6daf80a7237 | 168 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 169 | mode = STATE_A4; |
Morimoto448 | 5:b6daf80a7237 | 170 | fase = 4; |
Morimoto448 | 5:b6daf80a7237 | 171 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 172 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 173 | }else */ |
Morimoto448 | 5:b6daf80a7237 | 174 | if (timer > 15.0 && timer < 16.0){ |
Morimoto448 | 5:b6daf80a7237 | 175 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 176 | mode = STATE_A3; |
Morimoto448 | 5:b6daf80a7237 | 177 | fase = 4; |
Morimoto448 | 5:b6daf80a7237 | 178 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 179 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 180 | } |
Morimoto448 | 5:b6daf80a7237 | 181 | break; |
Morimoto448 | 5:b6daf80a7237 | 182 | |
Morimoto448 | 5:b6daf80a7237 | 183 | case 4: |
Morimoto448 | 5:b6daf80a7237 | 184 | /*if(measL <0.2 || measM <0.2 || measR <0.2){ |
Morimoto448 | 5:b6daf80a7237 | 185 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 186 | mode = STATE_C; |
Morimoto448 | 5:b6daf80a7237 | 187 | fase = 5; |
Morimoto448 | 5:b6daf80a7237 | 188 | }*/ |
Morimoto448 | 5:b6daf80a7237 | 189 | if(dist1 > 45 && dist1 < 100){ |
Morimoto448 | 5:b6daf80a7237 | 190 | fase = 5;} |
Morimoto448 | 5:b6daf80a7237 | 191 | |
Morimoto448 | 5:b6daf80a7237 | 192 | break; |
Morimoto448 | 5:b6daf80a7237 | 193 | case 5: |
Morimoto448 | 5:b6daf80a7237 | 194 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 195 | mode = STATE_C; |
Morimoto448 | 5:b6daf80a7237 | 196 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 197 | if(timer > 2.0 && dist1 > 60 && dist1 < 250){ |
Morimoto448 | 5:b6daf80a7237 | 198 | t.stop(); |
Morimoto448 | 5:b6daf80a7237 | 199 | t.reset(); |
Morimoto448 | 5:b6daf80a7237 | 200 | fase = 6; |
Morimoto448 | 5:b6daf80a7237 | 201 | } |
Morimoto448 | 5:b6daf80a7237 | 202 | break; |
Morimoto448 | 5:b6daf80a7237 | 203 | |
Morimoto448 | 5:b6daf80a7237 | 204 | case 6: |
Morimoto448 | 5:b6daf80a7237 | 205 | t.start(); |
Morimoto448 | 5:b6daf80a7237 | 206 | if(timer <2.0){ |
Morimoto448 | 5:b6daf80a7237 | 207 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 208 | mode = STATE_A; |
Morimoto448 | 5:b6daf80a7237 | 209 | }else{ |
Morimoto448 | 5:b6daf80a7237 | 210 | wait_flag = 1; |
Morimoto448 | 5:b6daf80a7237 | 211 | mode = STATE_A4;} |
Morimoto448 | 5:b6daf80a7237 | 212 | break; |
Morimoto448 | 5:b6daf80a7237 | 213 | |
Morimoto448 | 5:b6daf80a7237 | 214 | |
Morimoto448 | 5:b6daf80a7237 | 215 | |
Morimoto448 | 5:b6daf80a7237 | 216 | |
Morimoto448 | 5:b6daf80a7237 | 217 | |
Morimoto448 | 5:b6daf80a7237 | 218 | |
Morimoto448 | 5:b6daf80a7237 | 219 | |
Morimoto448 | 5:b6daf80a7237 | 220 | |
Morimoto448 | 5:b6daf80a7237 | 221 | |
Morimoto448 | 5:b6daf80a7237 | 222 | /* else if(timer > 20.0 && timer < 21.0){ |
Morimoto448 | 3:dd3b7c3c327b | 223 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 224 | mode = STATE_A; |
Morimoto448 | 5:b6daf80a7237 | 225 | }else if(timer > 30.0 && timer < 31.0){ |
Morimoto448 | 3:dd3b7c3c327b | 226 | wait_flag = 1; |
Morimoto448 | 3:dd3b7c3c327b | 227 | mode = STATE_D; |
Morimoto448 | 5:b6daf80a7237 | 228 | }else if(timer > 40.0 && timer < 41.0){ |
Morimoto448 | 3:dd3b7c3c327b | 229 | wait_flag = 1; |
Morimoto448 | 4:04ca6a211b85 | 230 | mode = STATE_A; |
Morimoto448 | 5:b6daf80a7237 | 231 | }else if(timer > 50.0 && timer < 51.0){ |
Morimoto448 | 3:dd3b7c3c327b | 232 | wait_flag = 1; |
Morimoto448 | 4:04ca6a211b85 | 233 | mode = STATE_D;} |
Morimoto448 | 5:b6daf80a7237 | 234 | break; */ |
Morimoto448 | 3:dd3b7c3c327b | 235 | } |
Morimoto448 | 1:aabe6887447c | 236 | // 状態が切り替わるときは一時停止 |
Morimoto448 | 0:7e3b7c017977 | 237 | if(wait_flag==1) |
Morimoto448 | 0:7e3b7c017977 | 238 | { |
Morimoto448 | 0:7e3b7c017977 | 239 | // フラグの初期化 |
Morimoto448 | 0:7e3b7c017977 | 240 | wait_flag = 0; |
Morimoto448 | 0:7e3b7c017977 | 241 | myled = 0; // LED消灯 |
Morimoto448 | 0:7e3b7c017977 | 242 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 243 | mypwm4.pulsewidth_ms(80); |
Morimoto448 | 1:aabe6887447c | 244 | mypwm6.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 245 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 246 | mypwm2.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 247 | mypwm3.pulsewidth_ms(80); |
Morimoto448 | 0:7e3b7c017977 | 248 | // 500ms待機する |
Morimoto448 | 1:aabe6887447c | 249 | wait(0.5); |
Morimoto448 | 0:7e3b7c017977 | 250 | } |
Morimoto448 | 3:dd3b7c3c327b | 251 | |
Morimoto448 | 0:7e3b7c017977 | 252 | switch (mode) |
Morimoto448 | 0:7e3b7c017977 | 253 | { |
Morimoto448 | 0:7e3b7c017977 | 254 | // STATE_A : 前進(左:正転 右:正転) |
Morimoto448 | 0:7e3b7c017977 | 255 | case STATE_A: |
Morimoto448 | 0:7e3b7c017977 | 256 | myled = 1; // LED点灯 |
Morimoto448 | 5:b6daf80a7237 | 257 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 258 | mypwm4.pulsewidth_ms(40); |
Morimoto448 | 1:aabe6887447c | 259 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 260 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 261 | mypwm2.pulsewidth_ms(38); |
ishi777 | 2:f568ec2e6955 | 262 | mypwm3.pulsewidth_ms(0); |
ishi777 | 2:f568ec2e6955 | 263 | break; |
Morimoto448 | 4:04ca6a211b85 | 264 | // STATE_A2 : 右に曲がる(左:正転 右:正転) |
ishi777 | 2:f568ec2e6955 | 265 | case STATE_A2: |
ishi777 | 2:f568ec2e6955 | 266 | myled = 1; // LED点灯 |
Morimoto448 | 5:b6daf80a7237 | 267 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 268 | mypwm4.pulsewidth_ms(55); |
ishi777 | 2:f568ec2e6955 | 269 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 270 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 271 | mypwm2.pulsewidth_ms(38); |
Morimoto448 | 4:04ca6a211b85 | 272 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 4:04ca6a211b85 | 273 | break; |
Morimoto448 | 4:04ca6a211b85 | 274 | // STATE_A3 : hidariに曲がる(左:正転 右:正転) |
Morimoto448 | 4:04ca6a211b85 | 275 | case STATE_A3: |
Morimoto448 | 4:04ca6a211b85 | 276 | myled = 1; // LED点灯 |
Morimoto448 | 5:b6daf80a7237 | 277 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 278 | mypwm4.pulsewidth_ms(40); |
Morimoto448 | 5:b6daf80a7237 | 279 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 4:04ca6a211b85 | 280 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 281 | mypwm2.pulsewidth_ms(55); |
Morimoto448 | 5:b6daf80a7237 | 282 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 283 | break; |
Morimoto448 | 5:b6daf80a7237 | 284 | case STATE_A4: |
Morimoto448 | 5:b6daf80a7237 | 285 | myled = 1; // LED点灯 |
Morimoto448 | 5:b6daf80a7237 | 286 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 287 | mypwm4.pulsewidth_ms(50); |
Morimoto448 | 4:04ca6a211b85 | 288 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 289 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 290 | mypwm2.pulsewidth_ms(40); |
Morimoto448 | 5:b6daf80a7237 | 291 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 292 | break; |
Morimoto448 | 5:b6daf80a7237 | 293 | // STATE_B : 左旋回(左:正転 右:逆転) |
Morimoto448 | 5:b6daf80a7237 | 294 | case STATE_B: |
Morimoto448 | 5:b6daf80a7237 | 295 | myled = 0; // LED点灯 |
Morimoto448 | 4:04ca6a211b85 | 296 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 297 | mypwm4.pulsewidth_ms(30); |
Morimoto448 | 5:b6daf80a7237 | 298 | mypwm6.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 299 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 300 | mypwm2.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 301 | mypwm3.pulsewidth_ms(30); |
Morimoto448 | 0:7e3b7c017977 | 302 | break; |
Morimoto448 | 0:7e3b7c017977 | 303 | |
Morimoto448 | 5:b6daf80a7237 | 304 | // STATE_C : 右旋回(左:逆転 右:正転) |
Morimoto448 | 5:b6daf80a7237 | 305 | case STATE_C: |
Morimoto448 | 5:b6daf80a7237 | 306 | myled = 0; // LED点灯 |
Morimoto448 | 0:7e3b7c017977 | 307 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 308 | mypwm4.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 309 | mypwm6.pulsewidth_ms(40); |
Morimoto448 | 0:7e3b7c017977 | 310 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 311 | mypwm2.pulsewidth_ms(40); |
Morimoto448 | 0:7e3b7c017977 | 312 | mypwm3.pulsewidth_ms(0); |
Morimoto448 | 0:7e3b7c017977 | 313 | break; |
Morimoto448 | 0:7e3b7c017977 | 314 | |
Morimoto448 | 0:7e3b7c017977 | 315 | // STATE_D : 後退(左:逆転 右:逆転) |
Morimoto448 | 0:7e3b7c017977 | 316 | case STATE_D: |
Morimoto448 | 0:7e3b7c017977 | 317 | myled = 1; // LED点灯 |
Morimoto448 | 5:b6daf80a7237 | 318 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 319 | mypwm4.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 320 | mypwm6.pulsewidth_ms(50); |
Morimoto448 | 5:b6daf80a7237 | 321 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 322 | mypwm2.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 323 | mypwm3.pulsewidth_ms(48); |
Morimoto448 | 0:7e3b7c017977 | 324 | break; |
Morimoto448 | 0:7e3b7c017977 | 325 | |
Morimoto448 | 5:b6daf80a7237 | 326 | // STATE_D : 後退(左:逆転 右:逆転) |
Morimoto448 | 5:b6daf80a7237 | 327 | case STATE_D2: |
Morimoto448 | 5:b6daf80a7237 | 328 | myled = 1; // LED点灯 |
Morimoto448 | 5:b6daf80a7237 | 329 | // 右モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 330 | mypwm4.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 331 | mypwm6.pulsewidth_ms(47); |
Morimoto448 | 5:b6daf80a7237 | 332 | // 左モータの制御 |
Morimoto448 | 5:b6daf80a7237 | 333 | mypwm2.pulsewidth_ms(0); |
Morimoto448 | 5:b6daf80a7237 | 334 | mypwm3.pulsewidth_ms(48); |
Morimoto448 | 5:b6daf80a7237 | 335 | break; |
Morimoto448 | 0:7e3b7c017977 | 336 | // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ) |
Morimoto448 | 0:7e3b7c017977 | 337 | case STATE_E: |
Morimoto448 | 0:7e3b7c017977 | 338 | myled = 0; // LED消灯 |
Morimoto448 | 5:b6daf80a7237 | 339 | // 右モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 340 | mypwm4.pulsewidth_ms(60); |
Morimoto448 | 1:aabe6887447c | 341 | mypwm6.pulsewidth_ms(60); |
Morimoto448 | 5:b6daf80a7237 | 342 | // 左モータの制御 |
Morimoto448 | 0:7e3b7c017977 | 343 | mypwm2.pulsewidth_ms(60); |
Morimoto448 | 0:7e3b7c017977 | 344 | mypwm3.pulsewidth_ms(60); |
Morimoto448 | 0:7e3b7c017977 | 345 | break; |
Morimoto448 | 0:7e3b7c017977 | 346 | } |
Morimoto448 | 0:7e3b7c017977 | 347 | } |
Morimoto448 | 4:04ca6a211b85 | 348 | } |
Morimoto448 | 5:b6daf80a7237 | 349 |