![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ライントレーサー制御用のプログラムです.
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?
User | Revision | Line number | New 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 | } |