ハヤト タジリ
/
16BALLsenso_NoVector
アナログ出力ボールセンサー
main.cpp
- Committer:
- tajiri1999
- Date:
- 2018-08-26
- Revision:
- 1:2323498aeca2
- Parent:
- 0:4a3b6ebfc310
- Child:
- 2:7c4a8e9f7db8
File content as of revision 1:2323498aeca2:
#include "mbed.h" DigitalIn IR0(A2);//0度(右回り) DigitalIn IR1(A1);//22.5度 DigitalIn IR2(D13);//45度 DigitalIn IR3(D12);//6.5度 DigitalIn IR4(D11);//90度 DigitalIn IR5(D10);//112.5度 DigitalIn IR6(D9);//135度 DigitalIn IR7(D8);//157.5度 DigitalIn IR8(D7);//180度 DigitalIn IR9(D6);//202.5度 DigitalIn IR10(D5);//225度 DigitalIn IR11(D4);//247.5度 DigitalIn IR12(D3);//270 DigitalIn IR13(D2);//292.5 DigitalIn IR14(A7);//315 DigitalIn IR15(A6);//337.5度 //distance=0のときボール無し degree=0.5のときボールが真前 AnalogOut degree(A3);//1のときボール無し0が最近 AnalogOut distance(A4); const int rate = 500;//サンプリングレートの決定 double Ballmin[16]; double deg,Ave,dis; double min(double a, double b);//最小値関数 int main(){ while(1){ double Ball[16] = {0,0,0,0,0,0,0,0,0,0,0,0}; for(int i = 1; i < rate; i++){//サンプリング if(IR0 == 1){ Ball[0] += 1; } if(IR1 == 1){ Ball[1] += 1; } if(IR2 == 1){ Ball[2] += 1; } if(IR3 == 1){ Ball[3] += 1; } if(IR4 == 1){ Ball[4] += 1; } if(IR5 == 1){ Ball[5] += 1; } if(IR6 == 1){ Ball[6] += 1; } if(IR7 == 1){ Ball[7] += 1; } if(IR8 == 1){ Ball[8] += 1; } if(IR9 == 1){ Ball[9] += 1; } if(IR10 == 1){ Ball[10] += 1; } if(IR11 == 1){ Ball[11] += 1; } if(IR12 == 1){ Ball[12] += 1; } if(IR13 == 1){ Ball[13] += 1; } if(IR14 == 1){ Ball[14] += 1; } if(IR15 == 1){ Ball[15] += 1; } wait_us(600); } Ballmin[0] = min(Ball[0],Ball[1]); Ballmin[1] = min(Ball[2],Ballmin[0]); Ballmin[2] = min(Ball[3],Ballmin[1]); Ballmin[3] = min(Ball[4],Ballmin[2]); Ballmin[4] = min(Ball[5],Ballmin[3]); Ballmin[5] = min(Ball[6],Ballmin[4]); Ballmin[6] = min(Ball[7],Ballmin[5]); Ballmin[7] = min(Ball[8],Ballmin[6]); Ballmin[8] = min(Ball[9],Ballmin[7]); Ballmin[9] = min(Ball[10],Ballmin[8]); Ballmin[11]= min(Ball[11],Ballmin[9]);//ボールセンサーの中心からの距離 Ballmin[12]= min(Ball[12],Ballmin[11]); Ballmin[13]= min(Ball[13],Ballmin[12]); Ballmin[14]= min(Ball[14],Ballmin[13]); Ballmin[15]= min(Ball[15],Ballmin[14]); Ave = Ballmin[14]/rate; if(Ave <= 0.9){//ボールがあるときの閾値設定 distance = Ave; if(Ball[0] == Ballmin[15]){ degree = 0.50; } else if(Ball[1] == Ballmin[15]){//22.5 degree = 0.5625; } else if(Ball[2] == Ballmin[15]){//45 degree = 0.625; } else if(Ball[3] == Ballmin[15]){//67.5 degree = 0.6875; } else if(Ball[4] == Ballmin[15]){//90 degree = 0.75; } else if(Ball[5] == Ballmin[15]){//112.5 degree = 0.8125; } else if(Ball[6] == Ballmin[15]){//135 degree =0.875; } else if(Ball[7] == Ballmin[15]){//157.5 degree = 0.9375; } else if(Ball[8] == Ballmin[15]){//180(-180) if(Ball[7]<= Ball[9]){ degree = 1; } else{ degree = 0; } } else if(Ball[9] == Ballmin[15]){//-157.5 degree = 0.0625; } else if(Ball[10] == Ballmin[15]){//-135 degree = 0.125; } else if(Ball[11] == Ballmin[15]){//-112.5 degree = 0.1875; } else if(Ball[12]== Ballmin[15]){//-90 degree = 0.25; } else if(Ball[13]== Ballmin[15]){//-66.5 degree = 0.3125; } else if(Ball[14]== Ballmin[15]){//-45 degree = 0.375; } else if(Ball[15]==Ballmin[15]){ degree = 0.4375; } } else{//ボール無し degree = 0; distance = 1; } } } double min(double a,double b){ if(a < b){ return(a); } else{ return(b); } }