mech
/
Line_Trace_Pro_NEW11
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
main.cpp
- Committer:
- isshikiyuya
- Date:
- 2018-12-20
- Revision:
- 4:282471928d78
- Parent:
- 3:55588b59ab29
- Child:
- 5:8a0721428fa7
File content as of revision 4:282471928d78:
#include "function.h"
PwmOut pwm11(D5); //左正転
PwmOut pwm12(D1); //左逆転
PwmOut pwm21(D6); //右正転
PwmOut pwm22(D9); //右逆転
DigitalOut LED(D2);
InterruptIn button1(A2); //フォトインタラプタ左
InterruptIn button2(A1); //フォトインタラプタ右
AnalogIn in1(A6); //フォトリフレクタ左
AnalogIn in2(A4); //フォトリフレクタ真ん中
AnalogIn in3(A3); //フォトリフレクタ右
float g = 0.0f;
float gain1 = 0.0f;
float gain2 = 0.0f;
float dif1 = 0.0f;
float dif2 = 0.0f;
float thr1 = 0.40f;
float thr2 = 0.40f;
float thr3 = 0.40f;
int judge1 = 0;
int judge2 = 0;
int judge3 = 0;
int main() {
pwm11.period(0.01f); //100Hz
pwm12.period(0.01f); //100Hz
pwm21.period(0.01f); //100Hz
pwm22.period(0.01f); //100Hz
button1.rise(&flip1);
button2.rise(&flip2);
int n;//3biteで状態を表現
flipper.attach(&flip3,0.05);//0.１秒ごとに回転数cを更新
while(1) {
if( led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
LED = 1.0f;
wait(0.05f);
LED = 0;
led_count1 = 0;
led_count2 = 0;
}
//////////////////floatのinをintのjudgeに変換
if(in1 > thr1){
judge1 = 1;
}
else{
judge1 = 0;
}
if(in2 > thr2){
judge2 = 1;
}
else{
judge2 = 0;
}
if(in3 > thr3){
judge3 = 1;
}
else{
judge3 = 0;
}
/////////////////
n = judge1 + judge2*2 + judge3*4;
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;
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;
}
}
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;
}
}
}
}