mech / Mbed 2 deprecated 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@0:7a6b6c687fa3, 2018-12-17 (annotated)

Committer:
isshikiyuya
Date:
Mon Dec 17 05:54:30 2018 +0000
Revision:
0:7a6b6c687fa3
Child:
1:f6b396987546
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
isshikiyuya 0:7a6b6c687fa3 1 #include "function.h"
isshikiyuya 0:7a6b6c687fa3 2
isshikiyuya 0:7a6b6c687fa3 3 PwmOut pwm11(D5); //左正転
isshikiyuya 0:7a6b6c687fa3 4 PwmOut pwm12(D1); //左逆転
isshikiyuya 0:7a6b6c687fa3 5 PwmOut pwm21(D6); //右正転
isshikiyuya 0:7a6b6c687fa3 6 PwmOut pwm22(D9); //右逆転
isshikiyuya 0:7a6b6c687fa3 7 DigitalOut LED(D2);
isshikiyuya 0:7a6b6c687fa3 8 InterruptIn button1(A2); //フォトインタラプタ左
isshikiyuya 0:7a6b6c687fa3 9 InterruptIn button2(A1); //フォトインタラプタ右
isshikiyuya 0:7a6b6c687fa3 10 AnalogIn in1(A6); //フォトリフレクタ左
isshikiyuya 0:7a6b6c687fa3 11 AnalogIn in2(A4); //フォトリフレクタ真ん中
isshikiyuya 0:7a6b6c687fa3 12 AnalogIn in3(A3); //フォトリフレクタ右
isshikiyuya 0:7a6b6c687fa3 13
isshikiyuya 0:7a6b6c687fa3 14 float g = 0.0f;
isshikiyuya 0:7a6b6c687fa3 15 float gain1 = 0.0f;
isshikiyuya 0:7a6b6c687fa3 16 float gain2 = 0.0f;
isshikiyuya 0:7a6b6c687fa3 17 float dif1 = 0.0f;
isshikiyuya 0:7a6b6c687fa3 18 float dif2 = 0.0f;
isshikiyuya 0:7a6b6c687fa3 19 float thr1 = 0.40f;
isshikiyuya 0:7a6b6c687fa3 20 float thr2 = 0.40f;
isshikiyuya 0:7a6b6c687fa3 21 float thr3 = 0.40f;
isshikiyuya 0:7a6b6c687fa3 22 int judge1 = 0;
isshikiyuya 0:7a6b6c687fa3 23 int judge2 = 0;
isshikiyuya 0:7a6b6c687fa3 24 int judge3 = 0;
isshikiyuya 0:7a6b6c687fa3 25
isshikiyuya 0:7a6b6c687fa3 26 int main() {
isshikiyuya 0:7a6b6c687fa3 27 pwm11.period(0.01f); //100Hz
isshikiyuya 0:7a6b6c687fa3 28 pwm12.period(0.01f); //100Hz
isshikiyuya 0:7a6b6c687fa3 29 pwm21.period(0.01f); //100Hz
isshikiyuya 0:7a6b6c687fa3 30 pwm22.period(0.01f); //100Hz
isshikiyuya 0:7a6b6c687fa3 31 button1.rise(&flip1);
isshikiyuya 0:7a6b6c687fa3 32 button2.rise(&flip2);
isshikiyuya 0:7a6b6c687fa3 33 int n;//3biteで状態を表現
isshikiyuya 0:7a6b6c687fa3 34
isshikiyuya 0:7a6b6c687fa3 35 flipper.attach(&flip3,0.05);//0.１秒ごとに回転数cを更新
isshikiyuya 0:7a6b6c687fa3 36
isshikiyuya 0:7a6b6c687fa3 37 while(1) {
isshikiyuya 0:7a6b6c687fa3 38
isshikiyuya 0:7a6b6c687fa3 39 if( led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
isshikiyuya 0:7a6b6c687fa3 40 LED = 1.0f;
isshikiyuya 0:7a6b6c687fa3 41 wait(0.5f);
isshikiyuya 0:7a6b6c687fa3 42 LED = 0;
isshikiyuya 0:7a6b6c687fa3 43 led_count1 = 0;
isshikiyuya 0:7a6b6c687fa3 44 led_count2 = 0;
isshikiyuya 0:7a6b6c687fa3 45 }
isshikiyuya 0:7a6b6c687fa3 46 //////////////////floatのinをintのjudgeに変換
isshikiyuya 0:7a6b6c687fa3 47 if(in1 > thr1){
isshikiyuya 0:7a6b6c687fa3 48 judge1 = 1;
isshikiyuya 0:7a6b6c687fa3 49 }
isshikiyuya 0:7a6b6c687fa3 50 else{
isshikiyuya 0:7a6b6c687fa3 51 judge1 = 0;
isshikiyuya 0:7a6b6c687fa3 52 }
isshikiyuya 0:7a6b6c687fa3 53 if(in2 > thr2){
isshikiyuya 0:7a6b6c687fa3 54 judge2 = 1;
isshikiyuya 0:7a6b6c687fa3 55 }
isshikiyuya 0:7a6b6c687fa3 56 else{
isshikiyuya 0:7a6b6c687fa3 57 judge2 = 0;
isshikiyuya 0:7a6b6c687fa3 58 }
isshikiyuya 0:7a6b6c687fa3 59 if(in3 > thr3){
isshikiyuya 0:7a6b6c687fa3 60 judge3 = 1;
isshikiyuya 0:7a6b6c687fa3 61 }
isshikiyuya 0:7a6b6c687fa3 62 else{
isshikiyuya 0:7a6b6c687fa3 63 judge3 = 0;
isshikiyuya 0:7a6b6c687fa3 64 }
isshikiyuya 0:7a6b6c687fa3 65 /////////////////
isshikiyuya 0:7a6b6c687fa3 66 n = judge1 + judge2*2 + judge3*4;
isshikiyuya 0:7a6b6c687fa3 67 /* distance = (distance1 + distance2) / 2;
isshikiyuya 0:7a6b6c687fa3 68 if(ditance < 43*12*){
isshikiyuya 0:7a6b6c687fa3 69 */
isshikiyuya 0:7a6b6c687fa3 70 switch(n){
isshikiyuya 0:7a6b6c687fa3 71 case(2)://白黒白ゆっくり進む
isshikiyuya 0:7a6b6c687fa3 72 g = 500.0f;
isshikiyuya 0:7a6b6c687fa3 73 dif1 = g - c2;
isshikiyuya 0:7a6b6c687fa3 74 dif2 = g - c1;
isshikiyuya 0:7a6b6c687fa3 75 gain1 = 0.0006f;
isshikiyuya 0:7a6b6c687fa3 76 gain2 = 0.0006f;
isshikiyuya 0:7a6b6c687fa3 77 pwm11.write(0.45f + dif1*gain1);
isshikiyuya 0:7a6b6c687fa3 78 pwm12.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 79 pwm21.write(0.45f + dif2*gain2);
isshikiyuya 0:7a6b6c687fa3 80 pwm22.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 81 break;
isshikiyuya 0:7a6b6c687fa3 82
isshikiyuya 0:7a6b6c687fa3 83 case(0)://白白白バック
isshikiyuya 0:7a6b6c687fa3 84 g = 500.0f;
isshikiyuya 0:7a6b6c687fa3 85 dif1 = g - c1;
isshikiyuya 0:7a6b6c687fa3 86 dif2 = g - c2;
isshikiyuya 0:7a6b6c687fa3 87 gain1 = 0.001f;
isshikiyuya 0:7a6b6c687fa3 88 gain2 = 0.001f;
isshikiyuya 0:7a6b6c687fa3 89
isshikiyuya 0:7a6b6c687fa3 90 pwm11.write(0.10f);
isshikiyuya 0:7a6b6c687fa3 91 pwm12.write(0.27f +dif1*gain1);
isshikiyuya 0:7a6b6c687fa3 92 pwm21.write(0.10f);
isshikiyuya 0:7a6b6c687fa3 93 pwm22.write(0.27f +dif2*gain2);
isshikiyuya 0:7a6b6c687fa3 94 break;
isshikiyuya 0:7a6b6c687fa3 95
isshikiyuya 0:7a6b6c687fa3 96
isshikiyuya 0:7a6b6c687fa3 97
isshikiyuya 0:7a6b6c687fa3 98 case(1)://黒白白左回転かなり強
isshikiyuya 0:7a6b6c687fa3 99 g = 500.0f;
isshikiyuya 0:7a6b6c687fa3 100 dif1 = g - c1;
isshikiyuya 0:7a6b6c687fa3 101 dif2 = g - c2;
isshikiyuya 0:7a6b6c687fa3 102 gain1 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 103 gain2 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 104
isshikiyuya 0:7a6b6c687fa3 105 pwm11.write(0.30f + dif1*gain1);
isshikiyuya 0:7a6b6c687fa3 106 pwm12.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 107 pwm21.write(0.50f + dif2*gain2);
isshikiyuya 0:7a6b6c687fa3 108 pwm22.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 109 break;
isshikiyuya 0:7a6b6c687fa3 110
isshikiyuya 0:7a6b6c687fa3 111 case(4)://白白黒右回転かなり強
isshikiyuya 0:7a6b6c687fa3 112 g = 500.0f;
isshikiyuya 0:7a6b6c687fa3 113 dif1 = g - c1;
isshikiyuya 0:7a6b6c687fa3 114 dif2 = g - c2;
isshikiyuya 0:7a6b6c687fa3 115 gain1 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 116 gain2 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 117
isshikiyuya 0:7a6b6c687fa3 118 pwm11.write(0.50f + dif1*gain1);
isshikiyuya 0:7a6b6c687fa3 119 pwm12.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 120 pwm21.write(0.20f);
isshikiyuya 0:7a6b6c687fa3 121 pwm22.write(0.30f + dif2*gain2);
isshikiyuya 0:7a6b6c687fa3 122 break;
isshikiyuya 0:7a6b6c687fa3 123
isshikiyuya 0:7a6b6c687fa3 124 case(3)://黒黒白左回転強
isshikiyuya 0:7a6b6c687fa3 125 g = 500.0f;
isshikiyuya 0:7a6b6c687fa3 126 dif1 = g - c1;
isshikiyuya 0:7a6b6c687fa3 127 dif2 = g - c2;
isshikiyuya 0:7a6b6c687fa3 128 gain1 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 129 gain2 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 130
isshikiyuya 0:7a6b6c687fa3 131 pwm11.write(0.00f);
isshikiyuya 0:7a6b6c687fa3 132 pwm12.write(0.00f);
isshikiyuya 0:7a6b6c687fa3 133 pwm21.write(0.40f + dif2*gain2);
isshikiyuya 0:7a6b6c687fa3 134 pwm22.write(0.20f);
isshikiyuya 0:7a6b6c687fa3 135 break;
isshikiyuya 0:7a6b6c687fa3 136
isshikiyuya 0:7a6b6c687fa3 137 case(6)://白黒黒右回転強
isshikiyuya 0:7a6b6c687fa3 138 g = 500.0f;
isshikiyuya 0:7a6b6c687fa3 139 dif1 = g - c1;
isshikiyuya 0:7a6b6c687fa3 140 dif2 = g - c2;
isshikiyuya 0:7a6b6c687fa3 141 gain1 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 142 gain2 = 0.0002f;
isshikiyuya 0:7a6b6c687fa3 143
isshikiyuya 0:7a6b6c687fa3 144 pwm11.write(0.40f + dif1*gain1);
isshikiyuya 0:7a6b6c687fa3 145 pwm12.write(0.20f);
isshikiyuya 0:7a6b6c687fa3 146 pwm21.write(0.00f);
isshikiyuya 0:7a6b6c687fa3 147 pwm22.write(0.00f);
isshikiyuya 0:7a6b6c687fa3 148 break;
isshikiyuya 0:7a6b6c687fa3 149
isshikiyuya 0:7a6b6c687fa3 150 case(7)://黒黒黒ゆっくり
isshikiyuya 0:7a6b6c687fa3 151 dif1 = g - c1;
isshikiyuya 0:7a6b6c687fa3 152 dif2 = g - c2;
isshikiyuya 0:7a6b6c687fa3 153 gain1 = 0.0004f;
isshikiyuya 0:7a6b6c687fa3 154 gain2 = 0.0004f;
isshikiyuya 0:7a6b6c687fa3 155
isshikiyuya 0:7a6b6c687fa3 156 pwm11.write(0.45f + dif1*gain1);
isshikiyuya 0:7a6b6c687fa3 157 pwm12.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 158 pwm21.write(0.45f + dif2*gain2);
isshikiyuya 0:7a6b6c687fa3 159 pwm22.write(0.30f);
isshikiyuya 0:7a6b6c687fa3 160 break;
isshikiyuya 0:7a6b6c687fa3 161
isshikiyuya 0:7a6b6c687fa3 162 default://それ以外(多分ないけど)
isshikiyuya 0:7a6b6c687fa3 163 pwm11.write(0.10f);
isshikiyuya 0:7a6b6c687fa3 164 pwm12.write(0.70f);
isshikiyuya 0:7a6b6c687fa3 165 pwm21.write(0.10f);
isshikiyuya 0:7a6b6c687fa3 166 pwm22.write(0.70f);
isshikiyuya 0:7a6b6c687fa3 167 break;
isshikiyuya 0:7a6b6c687fa3 168
isshikiyuya 0:7a6b6c687fa3 169 }
isshikiyuya 0:7a6b6c687fa3 170 }
isshikiyuya 0:7a6b6c687fa3 171 }