mech
/
Line_Trace_Pro_NEW
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Revision 4:282471928d78, committed 2018-12-20
- Comitter:
- isshikiyuya
- Date:
- Thu Dec 20 04:11:51 2018 +0000
- Parent:
- 3:55588b59ab29
- Commit message:
- NEW
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 55588b59ab29 -r 282471928d78 main.cpp
--- a/main.cpp Thu Dec 20 03:19:50 2018 +0000
+++ b/main.cpp Thu Dec 20 04:11:51 2018 +0000
@@ -35,15 +35,14 @@
flipper.attach(&flip3,0.05);//0.１秒ごとに回転数cを更新
while(1) {
- /*
if( led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
LED = 1.0f;
- wait(0.5f);
+ wait(0.05f);
LED = 0;
led_count1 = 0;
led_count2 = 0;
}
- */
+
//////////////////floatのinをintのjudgeに変換
if(in1 > thr1){
judge1 = 1;
@@ -65,9 +64,113 @@
}
/////////////////
n = judge1 + judge2*2 + judge3*4;
- /* distance = (distance1 + distance2) / 2;
- if(ditance < 43*12*){
- */
+ distance = (distance1 + distance2) / 2;
+ if(distance < 43*12*10 ){
+
+ switch(n){
+ case(2)://白黒白ゆっくり進む
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0006f;
+ gain2 = 0.0006f;
+ pwm11.write(0.80f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.80f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ case(0)://白白白バック
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0018f;
+ gain2 = 0.0018f;
+
+ pwm11.write(0.10f);
+ pwm12.write(0.30f +dif1*gain1);
+ pwm21.write(0.10f);
+ pwm22.write(0.30f +dif2*gain2);
+ break;
+
+
+
+ case(1)://黒白白左回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.70f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(4)://白白黒右回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.70f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f + dif2*gain2);
+ break;
+
+ case(3)://黒黒白左回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f);
+ pwm12.write(0.20f);
+ pwm21.write(0.70f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(6)://白黒黒右回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.70f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f);
+ break;
+
+ case(7)://黒黒黒ゆっくり
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0002f;
+ gain2 = 0.0002f;
+
+ pwm11.write(0.80f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.80f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ default://それ以外(多分ないけど)
+ pwm11.write(0.10f);
+ pwm12.write(0.50f);
+ pwm21.write(0.10f);
+ pwm22.write(0.50f);
+ break;
+
+ }
+ }
+
+ else if(43*12*10< distance && distance< 43*12*13 ){
+
switch(n){
case(2)://白黒白ゆっくり進む
g = 500.0f;
@@ -85,8 +188,8 @@
g = 500.0f;
dif1 = g - c1;
dif2 = g - c2;
- gain1 = 0.0012f;
- gain2 = 0.0012f;
+ gain1 = 0.0018f;
+ gain2 = 0.0018f;
pwm11.write(0.10f);
pwm12.write(0.30f +dif1*gain1);
@@ -169,4 +272,318 @@
}
}
-}
\ No newline at end of file
+
+ else if(43*12*13 < distance && distance < 43*12*18){
+ switch(n){
+ case(2)://白黒白ゆっくり進む
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.001f;
+ gain2 = 0.001f;
+ pwm11.write(0.40f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.40f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ case(0)://白白白バック
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0018f;
+ gain2 = 0.0018f;
+
+ pwm11.write(0.10f);
+ pwm12.write(0.30f +dif1*gain1);
+ pwm21.write(0.10f);
+ pwm22.write(0.30f +dif2*gain2);
+ break;
+
+
+
+ case(1)://黒白白左回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.50f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(4)://白白黒右回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.50f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f + dif2*gain2);
+ break;
+
+ case(3)://黒黒白左回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f);
+ pwm12.write(0.20f);
+ pwm21.write(0.50f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(6)://白黒黒右回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.50f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f);
+ break;
+
+ case(7)://黒黒黒ゆっくり
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.001f;
+ gain2 = 0.001f;
+
+ pwm11.write(0.40f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.40f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ default://それ以外(多分ないけど)
+ pwm11.write(0.10f);
+ pwm12.write(0.50f);
+ pwm21.write(0.10f);
+ pwm22.write(0.50f);
+ break;
+ }
+ }
+
+
+ else if(43*12*18 < distance && distance < 43*12*24){
+ switch(n){
+ case(2)://白黒白ゆっくり進む
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+ pwm11.write(0.80f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.80f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ case(0)://白白白バック
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0018f;
+ gain2 = 0.0018f;
+
+ pwm11.write(0.10f);
+ pwm12.write(0.30f +dif1*gain1);
+ pwm21.write(0.10f);
+ pwm22.write(0.30f +dif2*gain2);
+ break;
+
+
+
+ case(1)://黒白白左回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.50f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(4)://白白黒右回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.50f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f + dif2*gain2);
+ break;
+
+ case(3)://黒黒白左回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f);
+ pwm12.write(0.20f);
+ pwm21.write(0.50f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(6)://白黒黒右回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.50f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f);
+ break;
+
+ case(7)://黒黒黒ゆっくり
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.001f;
+ gain2 = 0.001f;
+
+ pwm11.write(0.80f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.80f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ default://それ以外(多分ないけど)
+ pwm11.write(0.10f);
+ pwm12.write(0.50f);
+ pwm21.write(0.10f);
+ pwm22.write(0.50f);
+ break;
+ }
+ }
+
+
+
+ else if(distance>43*12*24){
+
+ switch(n){
+ case(2)://白黒白ゆっくり進む
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0006f;
+ gain2 = 0.0006f;
+ pwm11.write(0.50f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.50f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ case(0)://白白白バック
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0018f;
+ gain2 = 0.0018f;
+
+ pwm11.write(0.10f);
+ pwm12.write(0.30f +dif1*gain1);
+ pwm21.write(0.10f);
+ pwm22.write(0.30f +dif2*gain2);
+ break;
+
+
+
+ case(1)://黒白白左回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.70f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(4)://白白黒右回転かなり強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.70f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f + dif2*gain2);
+ break;
+
+ case(3)://黒黒白左回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.20f);
+ pwm12.write(0.20f);
+ pwm21.write(0.70f + dif2*gain2);
+ pwm22.write(0.20f);
+ break;
+
+ case(6)://白黒黒右回転強
+ g = 500.0f;
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0007f;
+ gain2 = 0.0007f;
+
+ pwm11.write(0.70f + dif1*gain1);
+ pwm12.write(0.20f);
+ pwm21.write(0.20f);
+ pwm22.write(0.20f);
+ break;
+
+ case(7)://黒黒黒ゆっくり
+ dif1 = g - c1;
+ dif2 = g - c2;
+ gain1 = 0.0002f;
+ gain2 = 0.0002f;
+
+ pwm11.write(0.50f + dif1*gain1);
+ pwm12.write(0.30f);
+ pwm21.write(0.50f + dif2*gain2);
+ pwm22.write(0.30f);
+ break;
+
+ default://それ以外(多分ないけど)
+ pwm11.write(0.10f);
+ pwm12.write(0.50f);
+ pwm21.write(0.10f);
+ pwm22.write(0.50f);
+ break;
+
+ }
+ }
+
+
+ }
+ }
\ No newline at end of file