mech / Mbed OS Shiken_3

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.

main.cpp@6:9bd1da494e0b, 2018-12-18 (annotated)

Committer:
NSugiura
Date:
Tue Dec 18 19:04:49 2018 +0000
Revision:
6:9bd1da494e0b
Parent:
5:237c49800e14
Child:
7:de699a50fc63
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
isshikiyuya 4:45b45e8ca173 1 #include "function.h"
isshikiyuya 4:45b45e8ca173 2
isshikiyuya 4:45b45e8ca173 3 PwmOut pwm11(D5); //左正転
isshikiyuya 4:45b45e8ca173 4 PwmOut pwm12(D1); //左逆転
isshikiyuya 4:45b45e8ca173 5 PwmOut pwm21(D6); //右正転
isshikiyuya 4:45b45e8ca173 6 PwmOut pwm22(D9); //右逆転
isshikiyuya 4:45b45e8ca173 7 DigitalOut LED(D2);
isshikiyuya 4:45b45e8ca173 8 InterruptIn button1(A2); //フォトインタラプタ左
isshikiyuya 4:45b45e8ca173 9 InterruptIn button2(A1); //フォトインタラプタ右
isshikiyuya 4:45b45e8ca173 10 AnalogIn in1(A6); //フォトリフレクタ左
isshikiyuya 4:45b45e8ca173 11 AnalogIn in2(A4); //フォトリフレクタ真ん中
isshikiyuya 4:45b45e8ca173 12 AnalogIn in3(A3); //フォトリフレクタ右
isshikiyuya 4:45b45e8ca173 13
NSugiura 6:9bd1da494e0b 14 float g1 = 0.0f;//目標値（パルス数）
NSugiura 6:9bd1da494e0b 15 float g2 = 0.0f;//目標値（パルス数）
isshikiyuya 4:45b45e8ca173 16 float gain1 = 0.0f;//左ゲイン
isshikiyuya 4:45b45e8ca173 17 float gain2 = 0.0f;//右ゲイン
isshikiyuya 4:45b45e8ca173 18 float dif1 = 0.0f;//目標との差左
isshikiyuya 4:45b45e8ca173 19 float dif2 = 0.0f;//目標との差右
isshikiyuya 5:237c49800e14 20 float thr1 = 0.40f;//フォトリフレクタのしきい値左
isshikiyuya 5:237c49800e14 21 float thr2 = 0.40f;//フォトリフレクタのしきい値真ん中
isshikiyuya 5:237c49800e14 22 float thr3 = 0.40f;//フォトリフレクタのしきい値右
isshikiyuya 4:45b45e8ca173 23 int judge1 = 0;
isshikiyuya 4:45b45e8ca173 24 int judge2 = 0;
isshikiyuya 4:45b45e8ca173 25 int judge3 = 0;
isshikiyuya 4:45b45e8ca173 26 int mode1 = 10;
isshikiyuya 4:45b45e8ca173 27 int mode2 = 2;
isshikiyuya 4:45b45e8ca173 28 int mode3 = 2;
NSugiura 6:9bd1da494e0b 29 int curve_num = 1;
isshikiyuya 4:45b45e8ca173 30
isshikiyuya 4:45b45e8ca173 31 int main() {
isshikiyuya 4:45b45e8ca173 32 pwm11.period(0.01f); //100Hz
isshikiyuya 4:45b45e8ca173 33 pwm12.period(0.01f); //100Hz
isshikiyuya 4:45b45e8ca173 34 pwm21.period(0.01f); //100Hz
isshikiyuya 4:45b45e8ca173 35 pwm22.period(0.01f); //100Hz
isshikiyuya 4:45b45e8ca173 36 button1.rise(&flip1);
isshikiyuya 4:45b45e8ca173 37 button2.rise(&flip2);
isshikiyuya 4:45b45e8ca173 38 int n;//3biteで状態を表現
isshikiyuya 4:45b45e8ca173 39
isshikiyuya 4:45b45e8ca173 40 flipper.attach(&flip3,0.05f);//0.05秒ごとに回転数cを更新
isshikiyuya 4:45b45e8ca173 41
isshikiyuya 4:45b45e8ca173 42
isshikiyuya 4:45b45e8ca173 43 while(1) {
isshikiyuya 4:45b45e8ca173 44 distance = (distance1+distance2)/2;
isshikiyuya 4:45b45e8ca173 45
NSugiura 6:9bd1da494e0b 46 // if(distance < 43*2*12*mode1){
isshikiyuya 5:237c49800e14 47 /*
isshikiyuya 4:45b45e8ca173 48 if( led_count1 + led_count2 > 43*2*12){ //200mm毎にLEDを点滅
isshikiyuya 4:45b45e8ca173 49 LED = 1.0f;
isshikiyuya 4:45b45e8ca173 50 wait(0.5f);
isshikiyuya 4:45b45e8ca173 51 LED = 0;
isshikiyuya 4:45b45e8ca173 52 led_count1 = 0;
isshikiyuya 4:45b45e8ca173 53 led_count2 = 0;
isshikiyuya 4:45b45e8ca173 54 }
isshikiyuya 5:237c49800e14 55 */
isshikiyuya 4:45b45e8ca173 56 //////////////////floatのinをintのjudgeに変換
isshikiyuya 4:45b45e8ca173 57 if(in1 > thr1){
isshikiyuya 4:45b45e8ca173 58 judge1 = 1;
isshikiyuya 4:45b45e8ca173 59 }
isshikiyuya 4:45b45e8ca173 60 else{
isshikiyuya 4:45b45e8ca173 61 judge1 = 0;
isshikiyuya 4:45b45e8ca173 62 }
isshikiyuya 4:45b45e8ca173 63 if(in2 > thr2){
isshikiyuya 4:45b45e8ca173 64 judge2 = 1;
isshikiyuya 4:45b45e8ca173 65 }
isshikiyuya 4:45b45e8ca173 66 else{
isshikiyuya 4:45b45e8ca173 67 judge2 = 0;
isshikiyuya 4:45b45e8ca173 68 }
isshikiyuya 4:45b45e8ca173 69 if(in3 > thr3){
isshikiyuya 4:45b45e8ca173 70 judge3 = 1;
isshikiyuya 4:45b45e8ca173 71 }
isshikiyuya 4:45b45e8ca173 72 else{
isshikiyuya 4:45b45e8ca173 73 judge3 = 0;
isshikiyuya 4:45b45e8ca173 74 }
isshikiyuya 4:45b45e8ca173 75 /////////////////
isshikiyuya 4:45b45e8ca173 76 n = judge1 + judge2*2 + judge3*4;
isshikiyuya 4:45b45e8ca173 77
isshikiyuya 4:45b45e8ca173 78 switch(n){
isshikiyuya 4:45b45e8ca173 79 case(2)://白黒白進む
NSugiura 6:9bd1da494e0b 80 g1 = 500.0f;
NSugiura 6:9bd1da494e0b 81 g2 = 500.0f;
NSugiura 6:9bd1da494e0b 82 dif1 = g1 - c1;
NSugiura 6:9bd1da494e0b 83 dif2 = g2 - c2;
NSugiura 6:9bd1da494e0b 84
NSugiura 6:9bd1da494e0b 85 pwm11.write(0.35f);
NSugiura 6:9bd1da494e0b 86 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 87 pwm21.write(0.35f);
NSugiura 6:9bd1da494e0b 88 pwm22.write(0.10f);
NSugiura 6:9bd1da494e0b 89 break;
NSugiura 6:9bd1da494e0b 90
NSugiura 6:9bd1da494e0b 91
NSugiura 6:9bd1da494e0b 92 case(1)://黒白白左回転強
NSugiura 6:9bd1da494e0b 93
NSugiura 6:9bd1da494e0b 94 pwm11.write(0.30f);
NSugiura 6:9bd1da494e0b 95 pwm12.write(0.20f);
NSugiura 6:9bd1da494e0b 96 pwm21.write(0.38f);
NSugiura 6:9bd1da494e0b 97 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 98 break;
isshikiyuya 4:45b45e8ca173 99
NSugiura 6:9bd1da494e0b 100 case(3)://黒黒白左回転弱
NSugiura 6:9bd1da494e0b 101 pwm11.write(0.27f);
NSugiura 6:9bd1da494e0b 102 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 103 pwm21.write(0.33f);
NSugiura 6:9bd1da494e0b 104 pwm22.write(0.10f);
NSugiura 6:9bd1da494e0b 105 break;
NSugiura 6:9bd1da494e0b 106
NSugiura 6:9bd1da494e0b 107
NSugiura 6:9bd1da494e0b 108 case(4)://白白黒右回転強
isshikiyuya 5:237c49800e14 109 pwm11.write(0.40f);
NSugiura 6:9bd1da494e0b 110 pwm12.write(0.20f);
NSugiura 6:9bd1da494e0b 111 pwm21.write(0.30f);
NSugiura 6:9bd1da494e0b 112 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 113 break;
NSugiura 6:9bd1da494e0b 114
NSugiura 6:9bd1da494e0b 115
NSugiura 6:9bd1da494e0b 116 case(6)://白黒黒右回転弱
NSugiura 6:9bd1da494e0b 117 pwm11.write(0.33f);
NSugiura 6:9bd1da494e0b 118 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 119 pwm21.write(0.27f);
NSugiura 6:9bd1da494e0b 120 pwm22.write(0.10f);
NSugiura 6:9bd1da494e0b 121 break;
NSugiura 6:9bd1da494e0b 122
NSugiura 6:9bd1da494e0b 123 case(7)://黒黒黒爆走
NSugiura 6:9bd1da494e0b 124 pwm11.write(0.45f);
isshikiyuya 4:45b45e8ca173 125 pwm12.write(0.30f);
NSugiura 6:9bd1da494e0b 126 pwm21.write(0.45f);
isshikiyuya 4:45b45e8ca173 127 pwm22.write(0.30f);
isshikiyuya 4:45b45e8ca173 128 break;
isshikiyuya 4:45b45e8ca173 129
isshikiyuya 4:45b45e8ca173 130 case(0)://白白白
isshikiyuya 4:45b45e8ca173 131
NSugiura 6:9bd1da494e0b 132 switch(curve_num){
NSugiura 6:9bd1da494e0b 133 case(1):
NSugiura 6:9bd1da494e0b 134
NSugiura 6:9bd1da494e0b 135 pwm11.write(0.10f);
NSugiura 6:9bd1da494e0b 136 pwm12.write(0.20f);
NSugiura 6:9bd1da494e0b 137 pwm21.write(0.10f);
NSugiura 6:9bd1da494e0b 138 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 139
NSugiura 6:9bd1da494e0b 140 while(in2 < thr2){
NSugiura 6:9bd1da494e0b 141 pwm11.write(0.30f);
NSugiura 6:9bd1da494e0b 142 pwm12.write(0.30f);
NSugiura 6:9bd1da494e0b 143 pwm21.write(0.48f);
NSugiura 6:9bd1da494e0b 144 pwm22.write(0.30f);
NSugiura 6:9bd1da494e0b 145 }
NSugiura 6:9bd1da494e0b 146 curve_num++;
NSugiura 6:9bd1da494e0b 147
NSugiura 6:9bd1da494e0b 148 pwm11.write(0.40f);
NSugiura 6:9bd1da494e0b 149 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 150 pwm21.write(0.20f);
NSugiura 6:9bd1da494e0b 151 pwm22.write(0.45f);
NSugiura 6:9bd1da494e0b 152
NSugiura 6:9bd1da494e0b 153
NSugiura 6:9bd1da494e0b 154 break;
NSugiura 6:9bd1da494e0b 155
NSugiura 6:9bd1da494e0b 156 case(2):
NSugiura 6:9bd1da494e0b 157 pwm11.write(0.20f);
NSugiura 6:9bd1da494e0b 158 pwm12.write(0.40f);
NSugiura 6:9bd1da494e0b 159 pwm21.write(0.20f);
NSugiura 6:9bd1da494e0b 160 pwm22.write(0.40f);
NSugiura 6:9bd1da494e0b 161
NSugiura 6:9bd1da494e0b 162
NSugiura 6:9bd1da494e0b 163 while(in1 < thr1){
NSugiura 6:9bd1da494e0b 164 pwm11.write(1.00f);
NSugiura 6:9bd1da494e0b 165 pwm12.write(1.00f);
NSugiura 6:9bd1da494e0b 166 pwm21.write(0.40f);
NSugiura 6:9bd1da494e0b 167 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 168 }
NSugiura 6:9bd1da494e0b 169 curve_num++;
NSugiura 6:9bd1da494e0b 170
NSugiura 6:9bd1da494e0b 171 while(in2 > thr2){
NSugiura 6:9bd1da494e0b 172 pwm11.write(0.80f);
NSugiura 6:9bd1da494e0b 173 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 174 pwm21.write(0.20f);
NSugiura 6:9bd1da494e0b 175 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 176 }
NSugiura 6:9bd1da494e0b 177
NSugiura 6:9bd1da494e0b 178 break;
NSugiura 6:9bd1da494e0b 179 case(3):
NSugiura 6:9bd1da494e0b 180 while(in2 < thr2){
NSugiura 6:9bd1da494e0b 181 pwm11.write(0.45f);
NSugiura 6:9bd1da494e0b 182 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 183 pwm21.write(0.20f);
NSugiura 6:9bd1da494e0b 184 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 185 }
NSugiura 6:9bd1da494e0b 186 curve_num++;
NSugiura 6:9bd1da494e0b 187
NSugiura 6:9bd1da494e0b 188 while(in2 > thr2){
NSugiura 6:9bd1da494e0b 189 pwm11.write(0.90f);
NSugiura 6:9bd1da494e0b 190 pwm12.write(0.20f);
NSugiura 6:9bd1da494e0b 191 pwm21.write(0.20f);
NSugiura 6:9bd1da494e0b 192 pwm22.write(0.10f);
NSugiura 6:9bd1da494e0b 193 }
NSugiura 6:9bd1da494e0b 194
NSugiura 6:9bd1da494e0b 195 break;
NSugiura 6:9bd1da494e0b 196 case(4):
NSugiura 6:9bd1da494e0b 197 while(in1 < thr1){
NSugiura 6:9bd1da494e0b 198 pwm11.write(0.35f);
NSugiura 6:9bd1da494e0b 199 pwm12.write(0.10f);
NSugiura 6:9bd1da494e0b 200 pwm21.write(0.48f);
NSugiura 6:9bd1da494e0b 201 pwm22.write(0.20f);
NSugiura 6:9bd1da494e0b 202 }
NSugiura 6:9bd1da494e0b 203 curve_num++;
NSugiura 6:9bd1da494e0b 204 pwm11.write(0.20f);
NSugiura 6:9bd1da494e0b 205 pwm12.write(0.20f);
NSugiura 6:9bd1da494e0b 206 pwm21.write(0.40f);
NSugiura 6:9bd1da494e0b 207 pwm22.write(0.10f);
NSugiura 6:9bd1da494e0b 208
NSugiura 6:9bd1da494e0b 209 break;
NSugiura 6:9bd1da494e0b 210
NSugiura 6:9bd1da494e0b 211 default:
NSugiura 6:9bd1da494e0b 212 pwm11.write(0.30f);
NSugiura 6:9bd1da494e0b 213 pwm12.write(0.40f);
NSugiura 6:9bd1da494e0b 214 pwm21.write(0.30f);
NSugiura 6:9bd1da494e0b 215 pwm22.write(0.40f);
NSugiura 6:9bd1da494e0b 216 break;
NSugiura 6:9bd1da494e0b 217
NSugiura 6:9bd1da494e0b 218 }
NSugiura 6:9bd1da494e0b 219
isshikiyuya 4:45b45e8ca173 220 break;
isshikiyuya 4:45b45e8ca173 221
isshikiyuya 4:45b45e8ca173 222 default://それ以外(ないけど)
isshikiyuya 4:45b45e8ca173 223 pwm11.write(0.00f);
isshikiyuya 4:45b45e8ca173 224 pwm12.write(0.00f);
isshikiyuya 4:45b45e8ca173 225 pwm21.write(0.00f);
isshikiyuya 4:45b45e8ca173 226 pwm22.write(0.00f);
isshikiyuya 4:45b45e8ca173 227 break;
isshikiyuya 4:45b45e8ca173 228 }
NSugiura 6:9bd1da494e0b 229
NSugiura 6:9bd1da494e0b 230 /* else{
isshikiyuya 4:45b45e8ca173 231
isshikiyuya 4:45b45e8ca173 232 pwm11.write(0.00f);
isshikiyuya 4:45b45e8ca173 233 pwm12.write(0.00f);
isshikiyuya 4:45b45e8ca173 234 pwm21.write(0.00f);
isshikiyuya 4:45b45e8ca173 235 pwm22.write(0.00f);
NSugiura 6:9bd1da494e0b 236 }*/
isshikiyuya 4:45b45e8ca173 237 }
mbed_official 0:50d2b9c62765 238 }