廣智 角谷 / Mbed 2 deprecated LineTracer

ライントレーサー制御用のプログラムです．

Dependencies:   mbed

sensor.cpp@0:e9af471e2a2b, 2017-11-16 (annotated)

Committer:
Hirotomo777
Date:
Thu Nov 16 06:15:58 2017 +0000
Revision:
0:e9af471e2a2b
?7????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hirotomo777 0:e9af471e2a2b 1 #include "sensor.h"
Hirotomo777 0:e9af471e2a2b 2 #define threshold1 0.005
Hirotomo777 0:e9af471e2a2b 3 #define threshold2 0.005
Hirotomo777 0:e9af471e2a2b 4 /*
Hirotomo777 0:e9af471e2a2b 5 AnalogIn sensorfl(PTB0);
Hirotomo777 0:e9af471e2a2b 6 AnalogIn sensorfr(PTB1);
Hirotomo777 0:e9af471e2a2b 7 AnalogIn sensorbl(PTB2);
Hirotomo777 0:e9af471e2a2b 8 AnalogIn sensorbr(PTB3);
Hirotomo777 0:e9af471e2a2b 9 */
Hirotomo777 0:e9af471e2a2b 10 AnalogIn sensor1(PTB0); //左から数えて1,2,3,4
Hirotomo777 0:e9af471e2a2b 11 AnalogIn sensor2(PTB1);
Hirotomo777 0:e9af471e2a2b 12 AnalogIn sensor3(PTB2);
Hirotomo777 0:e9af471e2a2b 13 AnalogIn sensor4(PTB3);
Hirotomo777 0:e9af471e2a2b 14
Hirotomo777 0:e9af471e2a2b 15 DigitalIn sensorl(PTD6);
Hirotomo777 0:e9af471e2a2b 16 DigitalIn sensorr(PTD7);
Hirotomo777 0:e9af471e2a2b 17
Hirotomo777 0:e9af471e2a2b 18 Timer t;
Hirotomo777 0:e9af471e2a2b 19
Hirotomo777 0:e9af471e2a2b 20 static float th1 = 0.0;
Hirotomo777 0:e9af471e2a2b 21 static float th2 = 0.0;
Hirotomo777 0:e9af471e2a2b 22 static float th3 = 0.0;
Hirotomo777 0:e9af471e2a2b 23 static float th4 = 0.0;
Hirotomo777 0:e9af471e2a2b 24
Hirotomo777 0:e9af471e2a2b 25 void sensorInit(){
Hirotomo777 0:e9af471e2a2b 26 float th1max = sensor1;
Hirotomo777 0:e9af471e2a2b 27 float th1min = sensor1;
Hirotomo777 0:e9af471e2a2b 28 float th2max = sensor2;
Hirotomo777 0:e9af471e2a2b 29 float th2min = sensor2;
Hirotomo777 0:e9af471e2a2b 30 float th3max = sensor3;
Hirotomo777 0:e9af471e2a2b 31 float th3min = sensor3;
Hirotomo777 0:e9af471e2a2b 32 float th4max = sensor4;
Hirotomo777 0:e9af471e2a2b 33 float th4min = sensor4;
Hirotomo777 0:e9af471e2a2b 34 handle.pulsewidth(0.001);
Hirotomo777 0:e9af471e2a2b 35 t.start();
Hirotomo777 0:e9af471e2a2b 36 while(1){
Hirotomo777 0:e9af471e2a2b 37 if(t.read() > 1.0){
Hirotomo777 0:e9af471e2a2b 38 break;
Hirotomo777 0:e9af471e2a2b 39 }
Hirotomo777 0:e9af471e2a2b 40 if(th1max < sensor1){
Hirotomo777 0:e9af471e2a2b 41 th1max = sensor1;
Hirotomo777 0:e9af471e2a2b 42 }
Hirotomo777 0:e9af471e2a2b 43 if(th1min > sensor1){
Hirotomo777 0:e9af471e2a2b 44 th1min = sensor1;
Hirotomo777 0:e9af471e2a2b 45 }
Hirotomo777 0:e9af471e2a2b 46 if(th2max < sensor2){
Hirotomo777 0:e9af471e2a2b 47 th2max = sensor2;
Hirotomo777 0:e9af471e2a2b 48 }
Hirotomo777 0:e9af471e2a2b 49 if(th2min > sensor2){
Hirotomo777 0:e9af471e2a2b 50 th2min = sensor2;
Hirotomo777 0:e9af471e2a2b 51 }
Hirotomo777 0:e9af471e2a2b 52 if(th3max < sensor3){
Hirotomo777 0:e9af471e2a2b 53 th3max = sensor3;
Hirotomo777 0:e9af471e2a2b 54 }
Hirotomo777 0:e9af471e2a2b 55 if(th3min > sensor3){
Hirotomo777 0:e9af471e2a2b 56 th3min = sensor3;
Hirotomo777 0:e9af471e2a2b 57 }
Hirotomo777 0:e9af471e2a2b 58 if(th4max < sensor4){
Hirotomo777 0:e9af471e2a2b 59 th4max = sensor4;
Hirotomo777 0:e9af471e2a2b 60 }
Hirotomo777 0:e9af471e2a2b 61 if(th4min > sensor4){
Hirotomo777 0:e9af471e2a2b 62 th4min = sensor4;
Hirotomo777 0:e9af471e2a2b 63 }
Hirotomo777 0:e9af471e2a2b 64 }
Hirotomo777 0:e9af471e2a2b 65 //wait(1.0);
Hirotomo777 0:e9af471e2a2b 66 t.stop();
Hirotomo777 0:e9af471e2a2b 67 t.reset();
Hirotomo777 0:e9af471e2a2b 68 handle.pulsewidth(0.0021);
Hirotomo777 0:e9af471e2a2b 69 t.start();
Hirotomo777 0:e9af471e2a2b 70 while(1){
Hirotomo777 0:e9af471e2a2b 71 if(t.read() > 1.0){
Hirotomo777 0:e9af471e2a2b 72 break;
Hirotomo777 0:e9af471e2a2b 73 }
Hirotomo777 0:e9af471e2a2b 74 if(th1max < sensor1){
Hirotomo777 0:e9af471e2a2b 75 th1max = sensor1;
Hirotomo777 0:e9af471e2a2b 76 }
Hirotomo777 0:e9af471e2a2b 77 if(th1min > sensor1){
Hirotomo777 0:e9af471e2a2b 78 th1min = sensor1;
Hirotomo777 0:e9af471e2a2b 79 }
Hirotomo777 0:e9af471e2a2b 80 if(th2max < sensor2){
Hirotomo777 0:e9af471e2a2b 81 th2max = sensor2;
Hirotomo777 0:e9af471e2a2b 82 }
Hirotomo777 0:e9af471e2a2b 83 if(th2min > sensor2){
Hirotomo777 0:e9af471e2a2b 84 th2min = sensor2;
Hirotomo777 0:e9af471e2a2b 85 }
Hirotomo777 0:e9af471e2a2b 86 if(th3max < sensor3){
Hirotomo777 0:e9af471e2a2b 87 th3max = sensor3;
Hirotomo777 0:e9af471e2a2b 88 }
Hirotomo777 0:e9af471e2a2b 89 if(th3min > sensor3){
Hirotomo777 0:e9af471e2a2b 90 th3min = sensor3;
Hirotomo777 0:e9af471e2a2b 91 }
Hirotomo777 0:e9af471e2a2b 92 if(th4max < sensor4){
Hirotomo777 0:e9af471e2a2b 93 th4max = sensor4;
Hirotomo777 0:e9af471e2a2b 94 }
Hirotomo777 0:e9af471e2a2b 95 if(th4min > sensor4){
Hirotomo777 0:e9af471e2a2b 96 th4min = sensor4;
Hirotomo777 0:e9af471e2a2b 97 }
Hirotomo777 0:e9af471e2a2b 98 }
Hirotomo777 0:e9af471e2a2b 99 th1 = (th1max + th1min) / 2.0;
Hirotomo777 0:e9af471e2a2b 100 th2 = (th2max + th2min) / 2.0;
Hirotomo777 0:e9af471e2a2b 101 th3 = (th3max + th3min) / 2.0;
Hirotomo777 0:e9af471e2a2b 102 th4 = (th4max + th4min) / 2.0;
Hirotomo777 0:e9af471e2a2b 103 t.stop();
Hirotomo777 0:e9af471e2a2b 104 //wait(1.0);
Hirotomo777 0:e9af471e2a2b 105 handle.pulsewidth(0.00155);
Hirotomo777 0:e9af471e2a2b 106 wait(0.5);
Hirotomo777 0:e9af471e2a2b 107 }
Hirotomo777 0:e9af471e2a2b 108
Hirotomo777 0:e9af471e2a2b 109 int getReaction(){
Hirotomo777 0:e9af471e2a2b 110 int tmp = 0;
Hirotomo777 0:e9af471e2a2b 111 if(sensor1 < th1){
Hirotomo777 0:e9af471e2a2b 112 tmp += 0b100000;
Hirotomo777 0:e9af471e2a2b 113 }
Hirotomo777 0:e9af471e2a2b 114
Hirotomo777 0:e9af471e2a2b 115 if(sensor2 < th2){
Hirotomo777 0:e9af471e2a2b 116 //tmp += 0b010000;
Hirotomo777 0:e9af471e2a2b 117 tmp += 0b000000;
Hirotomo777 0:e9af471e2a2b 118 }
Hirotomo777 0:e9af471e2a2b 119 if(sensor3 < th3){
Hirotomo777 0:e9af471e2a2b 120 tmp += 0b000000;
Hirotomo777 0:e9af471e2a2b 121 //死んでる
Hirotomo777 0:e9af471e2a2b 122 }
Hirotomo777 0:e9af471e2a2b 123
Hirotomo777 0:e9af471e2a2b 124 if(sensor4 < th4){
Hirotomo777 0:e9af471e2a2b 125 tmp += 0b000100;
Hirotomo777 0:e9af471e2a2b 126 }
Hirotomo777 0:e9af471e2a2b 127
Hirotomo777 0:e9af471e2a2b 128 tmp += 0b000010 * sensorl;
Hirotomo777 0:e9af471e2a2b 129 tmp += 0b000001 * sensorr;
Hirotomo777 0:e9af471e2a2b 130 return tmp;
Hirotomo777 0:e9af471e2a2b 131 }