廣智 角谷
/
LineTracer
ライントレーサー制御用のプログラムです.
sensor.cpp
- Committer:
- Hirotomo777
- Date:
- 2017-11-16
- Revision:
- 0:e9af471e2a2b
File content as of revision 0:e9af471e2a2b:
#include "sensor.h" #define threshold1 0.005 #define threshold2 0.005 /* AnalogIn sensorfl(PTB0); AnalogIn sensorfr(PTB1); AnalogIn sensorbl(PTB2); AnalogIn sensorbr(PTB3); */ AnalogIn sensor1(PTB0); //左から数えて1,2,3,4 AnalogIn sensor2(PTB1); AnalogIn sensor3(PTB2); AnalogIn sensor4(PTB3); DigitalIn sensorl(PTD6); DigitalIn sensorr(PTD7); Timer t; static float th1 = 0.0; static float th2 = 0.0; static float th3 = 0.0; static float th4 = 0.0; void sensorInit(){ float th1max = sensor1; float th1min = sensor1; float th2max = sensor2; float th2min = sensor2; float th3max = sensor3; float th3min = sensor3; float th4max = sensor4; float th4min = sensor4; handle.pulsewidth(0.001); t.start(); while(1){ if(t.read() > 1.0){ break; } if(th1max < sensor1){ th1max = sensor1; } if(th1min > sensor1){ th1min = sensor1; } if(th2max < sensor2){ th2max = sensor2; } if(th2min > sensor2){ th2min = sensor2; } if(th3max < sensor3){ th3max = sensor3; } if(th3min > sensor3){ th3min = sensor3; } if(th4max < sensor4){ th4max = sensor4; } if(th4min > sensor4){ th4min = sensor4; } } //wait(1.0); t.stop(); t.reset(); handle.pulsewidth(0.0021); t.start(); while(1){ if(t.read() > 1.0){ break; } if(th1max < sensor1){ th1max = sensor1; } if(th1min > sensor1){ th1min = sensor1; } if(th2max < sensor2){ th2max = sensor2; } if(th2min > sensor2){ th2min = sensor2; } if(th3max < sensor3){ th3max = sensor3; } if(th3min > sensor3){ th3min = sensor3; } if(th4max < sensor4){ th4max = sensor4; } if(th4min > sensor4){ th4min = sensor4; } } th1 = (th1max + th1min) / 2.0; th2 = (th2max + th2min) / 2.0; th3 = (th3max + th3min) / 2.0; th4 = (th4max + th4min) / 2.0; t.stop(); //wait(1.0); handle.pulsewidth(0.00155); wait(0.5); } int getReaction(){ int tmp = 0; if(sensor1 < th1){ tmp += 0b100000; } if(sensor2 < th2){ //tmp += 0b010000; tmp += 0b000000; } if(sensor3 < th3){ tmp += 0b000000; //死んでる } if(sensor4 < th4){ tmp += 0b000100; } tmp += 0b000010 * sensorl; tmp += 0b000001 * sensorr; return tmp; }