ハヤト タジリ / Mbed 2 deprecated 16BALLsenso_NoVector

アナログ出力ボールセンサー

Dependencies:   mbed

main.cpp@2:7c4a8e9f7db8, 2018-08-26 (annotated)

Committer:
tajiri1999
Date:
Sun Aug 26 18:56:03 2018 +0000
Revision:
2:7c4a8e9f7db8
Parent:
1:2323498aeca2 
2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tajiri1999 0:4a3b6ebfc310 1 #include "mbed.h"
tajiri1999 0:4a3b6ebfc310 2
tajiri1999 0:4a3b6ebfc310 3
tajiri1999 0:4a3b6ebfc310 4
tajiri1999 0:4a3b6ebfc310 5 DigitalIn IR0(A2);//0度(右回り)
tajiri1999 0:4a3b6ebfc310 6 DigitalIn IR1(A1);//22.5度
tajiri1999 0:4a3b6ebfc310 7 DigitalIn IR2(D13);//45度
tajiri1999 0:4a3b6ebfc310 8 DigitalIn IR3(D12);//6.5度
tajiri1999 0:4a3b6ebfc310 9 DigitalIn IR4(D11);//90度
tajiri1999 0:4a3b6ebfc310 10 DigitalIn IR5(D10);//112.5度
tajiri1999 0:4a3b6ebfc310 11 DigitalIn IR6(D9);//135度
tajiri1999 0:4a3b6ebfc310 12 DigitalIn IR7(D8);//157.5度
tajiri1999 0:4a3b6ebfc310 13 DigitalIn IR8(D7);//180度
tajiri1999 0:4a3b6ebfc310 14 DigitalIn IR9(D6);//202.5度
tajiri1999 0:4a3b6ebfc310 15 DigitalIn IR10(D5);//225度
tajiri1999 0:4a3b6ebfc310 16 DigitalIn IR11(D4);//247.5度
tajiri1999 0:4a3b6ebfc310 17 DigitalIn IR12(D3);//270
tajiri1999 0:4a3b6ebfc310 18 DigitalIn IR13(D2);//292.5
tajiri1999 0:4a3b6ebfc310 19 DigitalIn IR14(A7);//315
tajiri1999 0:4a3b6ebfc310 20 DigitalIn IR15(A6);//337.5度
tajiri1999 0:4a3b6ebfc310 21
tajiri1999 0:4a3b6ebfc310 22 //distance=0のときボール無し degree=0.5のときボールが真前
tajiri1999 0:4a3b6ebfc310 23 AnalogOut degree(A3);//1のときボール無し0が最近
tajiri1999 0:4a3b6ebfc310 24 AnalogOut distance(A4);
tajiri1999 0:4a3b6ebfc310 25
tajiri1999 0:4a3b6ebfc310 26
tajiri1999 0:4a3b6ebfc310 27 const int rate = 500;//サンプリングレートの決定
tajiri1999 0:4a3b6ebfc310 28 double Ballmin[16];
tajiri1999 0:4a3b6ebfc310 29 double deg,Ave,dis;
tajiri1999 0:4a3b6ebfc310 30 double min(double a, double b);//最小値関数
tajiri1999 0:4a3b6ebfc310 31
tajiri1999 0:4a3b6ebfc310 32 int main(){
tajiri1999 0:4a3b6ebfc310 33 while(1){
tajiri1999 0:4a3b6ebfc310 34 double Ball[16] = {0,0,0,0,0,0,0,0,0,0,0,0};
tajiri1999 0:4a3b6ebfc310 35
tajiri1999 2:7c4a8e9f7db8 36 for(int i = 1; i <= rate; i++){//サンプリング
tajiri1999 0:4a3b6ebfc310 37 if(IR0 == 1){
tajiri1999 0:4a3b6ebfc310 38 Ball[0] += 1;
tajiri1999 0:4a3b6ebfc310 39 }
tajiri1999 0:4a3b6ebfc310 40 if(IR1 == 1){
tajiri1999 0:4a3b6ebfc310 41 Ball[1] += 1;
tajiri1999 0:4a3b6ebfc310 42 }
tajiri1999 0:4a3b6ebfc310 43 if(IR2 == 1){
tajiri1999 0:4a3b6ebfc310 44 Ball[2] += 1;
tajiri1999 0:4a3b6ebfc310 45 }
tajiri1999 0:4a3b6ebfc310 46 if(IR3 == 1){
tajiri1999 0:4a3b6ebfc310 47 Ball[3] += 1;
tajiri1999 0:4a3b6ebfc310 48 }
tajiri1999 0:4a3b6ebfc310 49 if(IR4 == 1){
tajiri1999 0:4a3b6ebfc310 50 Ball[4] += 1;
tajiri1999 0:4a3b6ebfc310 51 }
tajiri1999 0:4a3b6ebfc310 52 if(IR5 == 1){
tajiri1999 0:4a3b6ebfc310 53 Ball[5] += 1;
tajiri1999 0:4a3b6ebfc310 54 }
tajiri1999 0:4a3b6ebfc310 55 if(IR6 == 1){
tajiri1999 0:4a3b6ebfc310 56 Ball[6] += 1;
tajiri1999 0:4a3b6ebfc310 57 }
tajiri1999 0:4a3b6ebfc310 58 if(IR7 == 1){
tajiri1999 0:4a3b6ebfc310 59 Ball[7] += 1;
tajiri1999 0:4a3b6ebfc310 60 }
tajiri1999 0:4a3b6ebfc310 61 if(IR8 == 1){
tajiri1999 0:4a3b6ebfc310 62 Ball[8] += 1;
tajiri1999 0:4a3b6ebfc310 63 }
tajiri1999 0:4a3b6ebfc310 64 if(IR9 == 1){
tajiri1999 0:4a3b6ebfc310 65 Ball[9] += 1;
tajiri1999 0:4a3b6ebfc310 66 }
tajiri1999 0:4a3b6ebfc310 67 if(IR10 == 1){
tajiri1999 0:4a3b6ebfc310 68 Ball[10] += 1;
tajiri1999 0:4a3b6ebfc310 69 }
tajiri1999 0:4a3b6ebfc310 70 if(IR11 == 1){
tajiri1999 0:4a3b6ebfc310 71 Ball[11] += 1;
tajiri1999 0:4a3b6ebfc310 72 }
tajiri1999 0:4a3b6ebfc310 73 if(IR12 == 1){
tajiri1999 0:4a3b6ebfc310 74 Ball[12] += 1;
tajiri1999 0:4a3b6ebfc310 75 }
tajiri1999 0:4a3b6ebfc310 76 if(IR13 == 1){
tajiri1999 0:4a3b6ebfc310 77 Ball[13] += 1;
tajiri1999 0:4a3b6ebfc310 78 }
tajiri1999 0:4a3b6ebfc310 79 if(IR14 == 1){
tajiri1999 0:4a3b6ebfc310 80 Ball[14] += 1;
tajiri1999 0:4a3b6ebfc310 81 }
tajiri1999 0:4a3b6ebfc310 82 if(IR15 == 1){
tajiri1999 0:4a3b6ebfc310 83 Ball[15] += 1;
tajiri1999 0:4a3b6ebfc310 84 }
tajiri1999 1:2323498aeca2 85 wait_us(600);
tajiri1999 0:4a3b6ebfc310 86 }
tajiri1999 0:4a3b6ebfc310 87
tajiri1999 0:4a3b6ebfc310 88 Ballmin[0] = min(Ball[0],Ball[1]);
tajiri1999 0:4a3b6ebfc310 89 Ballmin[1] = min(Ball[2],Ballmin[0]);
tajiri1999 0:4a3b6ebfc310 90 Ballmin[2] = min(Ball[3],Ballmin[1]);
tajiri1999 0:4a3b6ebfc310 91 Ballmin[3] = min(Ball[4],Ballmin[2]);
tajiri1999 0:4a3b6ebfc310 92 Ballmin[4] = min(Ball[5],Ballmin[3]);
tajiri1999 0:4a3b6ebfc310 93 Ballmin[5] = min(Ball[6],Ballmin[4]);
tajiri1999 0:4a3b6ebfc310 94 Ballmin[6] = min(Ball[7],Ballmin[5]);
tajiri1999 0:4a3b6ebfc310 95 Ballmin[7] = min(Ball[8],Ballmin[6]);
tajiri1999 0:4a3b6ebfc310 96 Ballmin[8] = min(Ball[9],Ballmin[7]);
tajiri1999 0:4a3b6ebfc310 97 Ballmin[9] = min(Ball[10],Ballmin[8]);
tajiri1999 0:4a3b6ebfc310 98 Ballmin[11]= min(Ball[11],Ballmin[9]);//ボールセンサーの中心からの距離
tajiri1999 0:4a3b6ebfc310 99 Ballmin[12]= min(Ball[12],Ballmin[11]);
tajiri1999 0:4a3b6ebfc310 100 Ballmin[13]= min(Ball[13],Ballmin[12]);
tajiri1999 0:4a3b6ebfc310 101 Ballmin[14]= min(Ball[14],Ballmin[13]);
tajiri1999 0:4a3b6ebfc310 102 Ballmin[15]= min(Ball[15],Ballmin[14]);
tajiri1999 0:4a3b6ebfc310 103
tajiri1999 0:4a3b6ebfc310 104 Ave = Ballmin[14]/rate;
tajiri1999 0:4a3b6ebfc310 105 if(Ave <= 0.9){//ボールがあるときの閾値設定
tajiri1999 0:4a3b6ebfc310 106
tajiri1999 0:4a3b6ebfc310 107 distance = Ave;
tajiri1999 0:4a3b6ebfc310 108
tajiri1999 0:4a3b6ebfc310 109 if(Ball[0] == Ballmin[15]){
tajiri1999 0:4a3b6ebfc310 110 degree = 0.50;
tajiri1999 0:4a3b6ebfc310 111 }
tajiri1999 0:4a3b6ebfc310 112 else if(Ball[1] == Ballmin[15]){//22.5
tajiri1999 0:4a3b6ebfc310 113 degree = 0.5625;
tajiri1999 0:4a3b6ebfc310 114 }
tajiri1999 0:4a3b6ebfc310 115 else if(Ball[2] == Ballmin[15]){//45
tajiri1999 0:4a3b6ebfc310 116 degree = 0.625;
tajiri1999 0:4a3b6ebfc310 117 }
tajiri1999 0:4a3b6ebfc310 118 else if(Ball[3] == Ballmin[15]){//67.5
tajiri1999 0:4a3b6ebfc310 119 degree = 0.6875;
tajiri1999 0:4a3b6ebfc310 120 }
tajiri1999 0:4a3b6ebfc310 121 else if(Ball[4] == Ballmin[15]){//90
tajiri1999 0:4a3b6ebfc310 122 degree = 0.75;
tajiri1999 0:4a3b6ebfc310 123 }
tajiri1999 0:4a3b6ebfc310 124 else if(Ball[5] == Ballmin[15]){//112.5
tajiri1999 0:4a3b6ebfc310 125 degree = 0.8125;
tajiri1999 0:4a3b6ebfc310 126 }
tajiri1999 0:4a3b6ebfc310 127 else if(Ball[6] == Ballmin[15]){//135
tajiri1999 0:4a3b6ebfc310 128 degree =0.875;
tajiri1999 0:4a3b6ebfc310 129 }
tajiri1999 0:4a3b6ebfc310 130 else if(Ball[7] == Ballmin[15]){//157.5
tajiri1999 0:4a3b6ebfc310 131 degree = 0.9375;
tajiri1999 0:4a3b6ebfc310 132 }
tajiri1999 0:4a3b6ebfc310 133 else if(Ball[8] == Ballmin[15]){//180(-180)
tajiri1999 0:4a3b6ebfc310 134 if(Ball[7]<= Ball[9]){
tajiri1999 0:4a3b6ebfc310 135 degree = 1;
tajiri1999 0:4a3b6ebfc310 136 }
tajiri1999 0:4a3b6ebfc310 137 else{
tajiri1999 0:4a3b6ebfc310 138 degree = 0;
tajiri1999 0:4a3b6ebfc310 139 }
tajiri1999 0:4a3b6ebfc310 140 }
tajiri1999 0:4a3b6ebfc310 141 else if(Ball[9] == Ballmin[15]){//-157.5
tajiri1999 0:4a3b6ebfc310 142 degree = 0.0625;
tajiri1999 0:4a3b6ebfc310 143 }
tajiri1999 0:4a3b6ebfc310 144 else if(Ball[10] == Ballmin[15]){//-135
tajiri1999 0:4a3b6ebfc310 145 degree = 0.125;
tajiri1999 0:4a3b6ebfc310 146 }
tajiri1999 0:4a3b6ebfc310 147 else if(Ball[11] == Ballmin[15]){//-112.5
tajiri1999 0:4a3b6ebfc310 148 degree = 0.1875;
tajiri1999 0:4a3b6ebfc310 149 }
tajiri1999 0:4a3b6ebfc310 150 else if(Ball[12]== Ballmin[15]){//-90
tajiri1999 0:4a3b6ebfc310 151 degree = 0.25;
tajiri1999 0:4a3b6ebfc310 152 }
tajiri1999 0:4a3b6ebfc310 153 else if(Ball[13]== Ballmin[15]){//-66.5
tajiri1999 0:4a3b6ebfc310 154 degree = 0.3125;
tajiri1999 0:4a3b6ebfc310 155 }
tajiri1999 0:4a3b6ebfc310 156 else if(Ball[14]== Ballmin[15]){//-45
tajiri1999 0:4a3b6ebfc310 157 degree = 0.375;
tajiri1999 0:4a3b6ebfc310 158 }
tajiri1999 0:4a3b6ebfc310 159 else if(Ball[15]==Ballmin[15]){
tajiri1999 0:4a3b6ebfc310 160 degree = 0.4375;
tajiri1999 0:4a3b6ebfc310 161 }
tajiri1999 0:4a3b6ebfc310 162 }
tajiri1999 0:4a3b6ebfc310 163 else{//ボール無し
tajiri1999 0:4a3b6ebfc310 164 degree = 0;
tajiri1999 0:4a3b6ebfc310 165 distance = 1;
tajiri1999 0:4a3b6ebfc310 166 }
tajiri1999 0:4a3b6ebfc310 167 }
tajiri1999 0:4a3b6ebfc310 168 }
tajiri1999 0:4a3b6ebfc310 169
tajiri1999 0:4a3b6ebfc310 170 double min(double a,double b){
tajiri1999 0:4a3b6ebfc310 171 if(a < b){
tajiri1999 0:4a3b6ebfc310 172 return(a);
tajiri1999 0:4a3b6ebfc310 173 }
tajiri1999 0:4a3b6ebfc310 174 else{
tajiri1999 0:4a3b6ebfc310 175 return(b);
tajiri1999 0:4a3b6ebfc310 176 }
tajiri1999 0:4a3b6ebfc310 177 }