aaaa
Dependencies: ColorSensor1 Servo mbed
main.cpp@0:4c7c138f3891, 2013-10-19 (annotated)
- Committer:
- OGA
- Date:
- Sat Oct 19 04:50:26 2013 +0000
- Revision:
- 0:4c7c138f3891
10/19;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OGA | 0:4c7c138f3891 | 1 | //カラーセンサ6つ同時に動かすプログラム |
OGA | 0:4c7c138f3891 | 2 | //大会用ロボ |
OGA | 0:4c7c138f3891 | 3 | //ジャンプのプログラムも入ってる |
OGA | 0:4c7c138f3891 | 4 | //10月17日に使う |
OGA | 0:4c7c138f3891 | 5 | #include "mbed.h" |
OGA | 0:4c7c138f3891 | 6 | #include "ColorSensor.h" |
OGA | 0:4c7c138f3891 | 7 | #include "Servo.h" |
OGA | 0:4c7c138f3891 | 8 | |
OGA | 0:4c7c138f3891 | 9 | #include "main.h" |
OGA | 0:4c7c138f3891 | 10 | |
OGA | 0:4c7c138f3891 | 11 | |
OGA | 0:4c7c138f3891 | 12 | |
OGA | 0:4c7c138f3891 | 13 | |
OGA | 0:4c7c138f3891 | 14 | int cAve,roboF, jumI; |
OGA | 0:4c7c138f3891 | 15 | |
OGA | 0:4c7c138f3891 | 16 | void tic_sensor() |
OGA | 0:4c7c138f3891 | 17 | { |
OGA | 0:4c7c138f3891 | 18 | colorUpdate(1); |
OGA | 0:4c7c138f3891 | 19 | roboF = robotFront(); |
OGA | 0:4c7c138f3891 | 20 | jumI = jumpInstruction(roboF); |
OGA | 0:4c7c138f3891 | 21 | //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); |
OGA | 0:4c7c138f3891 | 22 | /*lcd.cls(); |
OGA | 0:4c7c138f3891 | 23 | lcd.locate(0,0); |
OGA | 0:4c7c138f3891 | 24 | lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); |
OGA | 0:4c7c138f3891 | 25 | */ |
OGA | 0:4c7c138f3891 | 26 | } |
OGA | 0:4c7c138f3891 | 27 | |
OGA | 0:4c7c138f3891 | 28 | |
OGA | 0:4c7c138f3891 | 29 | |
OGA | 0:4c7c138f3891 | 30 | ////////////////////////////////////////カラーセンサの//////////////////////////////////////// |
OGA | 0:4c7c138f3891 | 31 | ////////////////////////////////////////更新と補正プログラム/////////////////////////////////// |
OGA | 0:4c7c138f3891 | 32 | /*void rivisedate() |
OGA | 0:4c7c138f3891 | 33 | { |
OGA | 0:4c7c138f3891 | 34 | unsigned long red = 0,green = 0,blue =0; |
OGA | 0:4c7c138f3891 | 35 | static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; |
OGA | 0:4c7c138f3891 | 36 | |
OGA | 0:4c7c138f3891 | 37 | //最初の20回だけ平均を取る |
OGA | 0:4c7c138f3891 | 38 | for (int i=0;i<=20;i++){ |
OGA | 0:4c7c138f3891 | 39 | color0.getRGB(R[0],G[0],B[0]); |
OGA | 0:4c7c138f3891 | 40 | red += R[0] ; |
OGA | 0:4c7c138f3891 | 41 | green += G[0] ; |
OGA | 0:4c7c138f3891 | 42 | blue += B[0] ; |
OGA | 0:4c7c138f3891 | 43 | //pc.printf(" %d %d\n",ptm(sum),sum); |
OGA | 0:4c7c138f3891 | 44 | } |
OGA | 0:4c7c138f3891 | 45 | |
OGA | 0:4c7c138f3891 | 46 | rir = (double)green/ red ; |
OGA | 0:4c7c138f3891 | 47 | rib = (double)green/ blue ; |
OGA | 0:4c7c138f3891 | 48 | }*/ |
OGA | 0:4c7c138f3891 | 49 | |
OGA | 0:4c7c138f3891 | 50 | void colorUpdate(uint8_t mode) |
OGA | 0:4c7c138f3891 | 51 | { |
OGA | 0:4c7c138f3891 | 52 | double colorSum[COLOR_NUM]; |
OGA | 0:4c7c138f3891 | 53 | unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; |
OGA | 0:4c7c138f3891 | 54 | |
OGA | 0:4c7c138f3891 | 55 | |
OGA | 0:4c7c138f3891 | 56 | for (int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 57 | R[i] = 0; |
OGA | 0:4c7c138f3891 | 58 | G[i] = 0; |
OGA | 0:4c7c138f3891 | 59 | B[i] = 0; |
OGA | 0:4c7c138f3891 | 60 | redp[i] = 0; |
OGA | 0:4c7c138f3891 | 61 | greenp[i] = 0; |
OGA | 0:4c7c138f3891 | 62 | bluep[i] = 0; |
OGA | 0:4c7c138f3891 | 63 | } |
OGA | 0:4c7c138f3891 | 64 | |
OGA | 0:4c7c138f3891 | 65 | //カラーセンサの切り替え |
OGA | 0:4c7c138f3891 | 66 | if(mode == 1){ |
OGA | 0:4c7c138f3891 | 67 | color0.getRGB(R[0],G[0],B[0]); |
OGA | 0:4c7c138f3891 | 68 | color1.getRGB(R[1],G[1],B[1]); |
OGA | 0:4c7c138f3891 | 69 | color2.getRGB(R[2],G[2],B[2]); |
OGA | 0:4c7c138f3891 | 70 | |
OGA | 0:4c7c138f3891 | 71 | color3.getRGB(R[3],G[3],B[3]); |
OGA | 0:4c7c138f3891 | 72 | color4.getRGB(R[4],G[4],B[4]); |
OGA | 0:4c7c138f3891 | 73 | color5.getRGB(R[5],G[5],B[5]); |
OGA | 0:4c7c138f3891 | 74 | } |
OGA | 0:4c7c138f3891 | 75 | |
OGA | 0:4c7c138f3891 | 76 | /*for (int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 77 | colorSum[i] = R[i]*rir + G[i] + B[i]*rib ; |
OGA | 0:4c7c138f3891 | 78 | redp[i] = R[i]* rir * 100 / colorSum[i]; |
OGA | 0:4c7c138f3891 | 79 | greenp[i] = G[i] * 100 / colorSum[i]; |
OGA | 0:4c7c138f3891 | 80 | bluep[i] = B[i]* rib * 100 / colorSum[i]; |
OGA | 0:4c7c138f3891 | 81 | }*/ |
OGA | 0:4c7c138f3891 | 82 | |
OGA | 0:4c7c138f3891 | 83 | for (int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 84 | colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3; |
OGA | 0:4c7c138f3891 | 85 | redp[i] = R[i]*0.65 * 100 / colorSum[i]; |
OGA | 0:4c7c138f3891 | 86 | greenp[i] = G[i] * 100 / colorSum[i]; |
OGA | 0:4c7c138f3891 | 87 | bluep[i] = B[i]*1.3 * 100 / colorSum[i]; |
OGA | 0:4c7c138f3891 | 88 | } |
OGA | 0:4c7c138f3891 | 89 | } |
OGA | 0:4c7c138f3891 | 90 | |
OGA | 0:4c7c138f3891 | 91 | |
OGA | 0:4c7c138f3891 | 92 | uint16_t moving_ave(uint8_t num, uint16_t data)//移動平均的な |
OGA | 0:4c7c138f3891 | 93 | { |
OGA | 0:4c7c138f3891 | 94 | uint8_t threshold = 0; |
OGA | 0:4c7c138f3891 | 95 | static uint16_t tmp[COLOR_NUM][10] = {{0}}; |
OGA | 0:4c7c138f3891 | 96 | //static uint32_t sum[COLOR_NUM] = {0}; |
OGA | 0:4c7c138f3891 | 97 | |
OGA | 0:4c7c138f3891 | 98 | //sum[num] -= tmp[num][9]; |
OGA | 0:4c7c138f3891 | 99 | //sum[num] += data; |
OGA | 0:4c7c138f3891 | 100 | |
OGA | 0:4c7c138f3891 | 101 | for(uint8_t i=9; i>=1; i--){ |
OGA | 0:4c7c138f3891 | 102 | tmp[num][i] = tmp[num][i-1]; |
OGA | 0:4c7c138f3891 | 103 | } |
OGA | 0:4c7c138f3891 | 104 | tmp[num][0] = data; |
OGA | 0:4c7c138f3891 | 105 | |
OGA | 0:4c7c138f3891 | 106 | for(uint8_t i=0; i<=9; i++){ |
OGA | 0:4c7c138f3891 | 107 | if(tmp[num][i] >= B_THR){ |
OGA | 0:4c7c138f3891 | 108 | threshold++; |
OGA | 0:4c7c138f3891 | 109 | } |
OGA | 0:4c7c138f3891 | 110 | } |
OGA | 0:4c7c138f3891 | 111 | |
OGA | 0:4c7c138f3891 | 112 | return threshold; |
OGA | 0:4c7c138f3891 | 113 | } |
OGA | 0:4c7c138f3891 | 114 | |
OGA | 0:4c7c138f3891 | 115 | ////////////////////////////////////////ロボの状態/////////////////////////////////////////// |
OGA | 0:4c7c138f3891 | 116 | /////////////////////////////////////////////////////////////////////////////////////////// |
OGA | 0:4c7c138f3891 | 117 | uint8_t robotFront()//縄回しロボの正面にいるか |
OGA | 0:4c7c138f3891 | 118 | { |
OGA | 0:4c7c138f3891 | 119 | uint8_t threshold = 0; |
OGA | 0:4c7c138f3891 | 120 | |
OGA | 0:4c7c138f3891 | 121 | for(int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 122 | if(moving_ave(i, bluep[i]) >= 8) threshold++; |
OGA | 0:4c7c138f3891 | 123 | } |
OGA | 0:4c7c138f3891 | 124 | |
OGA | 0:4c7c138f3891 | 125 | |
OGA | 0:4c7c138f3891 | 126 | /*for(int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 127 | cAve = moving_ave(i, bluep[i]); |
OGA | 0:4c7c138f3891 | 128 | if(cAve >= 8) threshold++; |
OGA | 0:4c7c138f3891 | 129 | }*/ |
OGA | 0:4c7c138f3891 | 130 | |
OGA | 0:4c7c138f3891 | 131 | if(threshold >= 1){ |
OGA | 0:4c7c138f3891 | 132 | return 1; |
OGA | 0:4c7c138f3891 | 133 | }else{ |
OGA | 0:4c7c138f3891 | 134 | return 0; |
OGA | 0:4c7c138f3891 | 135 | } |
OGA | 0:4c7c138f3891 | 136 | } |
OGA | 0:4c7c138f3891 | 137 | |
OGA | 0:4c7c138f3891 | 138 | uint8_t jumpInstruction(uint8_t front)//ジャンプ命令 |
OGA | 0:4c7c138f3891 | 139 | { |
OGA | 0:4c7c138f3891 | 140 | uint8_t threshold = 0; |
OGA | 0:4c7c138f3891 | 141 | |
OGA | 0:4c7c138f3891 | 142 | if(front == 1){ |
OGA | 0:4c7c138f3891 | 143 | for(int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 144 | if(redp[i] >= R_THR) threshold++; |
OGA | 0:4c7c138f3891 | 145 | } |
OGA | 0:4c7c138f3891 | 146 | } |
OGA | 0:4c7c138f3891 | 147 | |
OGA | 0:4c7c138f3891 | 148 | if(threshold >= 1){ |
OGA | 0:4c7c138f3891 | 149 | return 1; |
OGA | 0:4c7c138f3891 | 150 | }else{ |
OGA | 0:4c7c138f3891 | 151 | return 0; |
OGA | 0:4c7c138f3891 | 152 | } |
OGA | 0:4c7c138f3891 | 153 | } |
OGA | 0:4c7c138f3891 | 154 | |
OGA | 0:4c7c138f3891 | 155 | |
OGA | 0:4c7c138f3891 | 156 | uint8_t robotoStop() |
OGA | 0:4c7c138f3891 | 157 | { |
OGA | 0:4c7c138f3891 | 158 | uint8_t threshold = 0; |
OGA | 0:4c7c138f3891 | 159 | |
OGA | 0:4c7c138f3891 | 160 | for(int i=0; i<COLOR_NUM; i++){ |
OGA | 0:4c7c138f3891 | 161 | if(bluep[i] >= B_THR) threshold++; |
OGA | 0:4c7c138f3891 | 162 | } |
OGA | 0:4c7c138f3891 | 163 | |
OGA | 0:4c7c138f3891 | 164 | if(threshold >= 1){ |
OGA | 0:4c7c138f3891 | 165 | return 1; |
OGA | 0:4c7c138f3891 | 166 | }else{ |
OGA | 0:4c7c138f3891 | 167 | return 0; |
OGA | 0:4c7c138f3891 | 168 | } |
OGA | 0:4c7c138f3891 | 169 | } |
OGA | 0:4c7c138f3891 | 170 | |
OGA | 0:4c7c138f3891 | 171 | |
OGA | 0:4c7c138f3891 | 172 | ////////////////////////////////////////ジャンププログラム//////////////////////////////////// |
OGA | 0:4c7c138f3891 | 173 | /////////////////////////////////////////////////////////////////////////////////////////// |
OGA | 0:4c7c138f3891 | 174 | /* |
OGA | 0:4c7c138f3891 | 175 | void jumping(uint8_t threshold) |
OGA | 0:4c7c138f3891 | 176 | { |
OGA | 0:4c7c138f3891 | 177 | //超音波でジャンプのタイミング合わせる |
OGA | 0:4c7c138f3891 | 178 | if(threshold >= 1){ |
OGA | 0:4c7c138f3891 | 179 | jump_t.reset(); |
OGA | 0:4c7c138f3891 | 180 | jump_t.start(); |
OGA | 0:4c7c138f3891 | 181 | while(ultrasonicVal[0] < 1700){ |
OGA | 0:4c7c138f3891 | 182 | led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0; |
OGA | 0:4c7c138f3891 | 183 | air[0] = 1; air[1] = 0; |
OGA | 0:4c7c138f3891 | 184 | |
OGA | 0:4c7c138f3891 | 185 | if(jump_t.read_ms() > 1000)break; |
OGA | 0:4c7c138f3891 | 186 | } |
OGA | 0:4c7c138f3891 | 187 | led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1; |
OGA | 0:4c7c138f3891 | 188 | air[0] = 0; air[1] = 1; |
OGA | 0:4c7c138f3891 | 189 | wait(0.5); |
OGA | 0:4c7c138f3891 | 190 | }else{ |
OGA | 0:4c7c138f3891 | 191 | led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; |
OGA | 0:4c7c138f3891 | 192 | } |
OGA | 0:4c7c138f3891 | 193 | }*/ |
OGA | 0:4c7c138f3891 | 194 | |
OGA | 0:4c7c138f3891 | 195 | /*void jumpAction(uint8_t threshold) |
OGA | 0:4c7c138f3891 | 196 | { |
OGA | 0:4c7c138f3891 | 197 | if(sw1){//フォトインタラプタがオフ |
OGA | 0:4c7c138f3891 | 198 | myservo1 = 0.3; led[0] = 0;led[1] = 1; led[2] = 0; led[3] = 0; |
OGA | 0:4c7c138f3891 | 199 | }else{//フォトインタラプタがオン |
OGA | 0:4c7c138f3891 | 200 | myservo1 = 0.5; led[0] = 1;led[1] = 0; led[2] = 0; led[3] = 0; |
OGA | 0:4c7c138f3891 | 201 | } |
OGA | 0:4c7c138f3891 | 202 | |
OGA | 0:4c7c138f3891 | 203 | if(threshold){ |
OGA | 0:4c7c138f3891 | 204 | led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1; |
OGA | 0:4c7c138f3891 | 205 | myservo1 = 0.3; |
OGA | 0:4c7c138f3891 | 206 | wait(0.2); |
OGA | 0:4c7c138f3891 | 207 | }else{ |
OGA | 0:4c7c138f3891 | 208 | //led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; |
OGA | 0:4c7c138f3891 | 209 | } |
OGA | 0:4c7c138f3891 | 210 | |
OGA | 0:4c7c138f3891 | 211 | }*/ |
OGA | 0:4c7c138f3891 | 212 | |
OGA | 0:4c7c138f3891 | 213 | |
OGA | 0:4c7c138f3891 | 214 | |
OGA | 0:4c7c138f3891 | 215 | |
OGA | 0:4c7c138f3891 | 216 | |
OGA | 0:4c7c138f3891 | 217 | |
OGA | 0:4c7c138f3891 | 218 | #define SEND_DATA_NUM 7 |
OGA | 0:4c7c138f3891 | 219 | #define RECEIVE_DATA_NUM 7 |
OGA | 0:4c7c138f3891 | 220 | |
OGA | 0:4c7c138f3891 | 221 | #define KEYCODE 120 |
OGA | 0:4c7c138f3891 | 222 | |
OGA | 0:4c7c138f3891 | 223 | Serial device(p28,p27); |
OGA | 0:4c7c138f3891 | 224 | |
OGA | 0:4c7c138f3891 | 225 | static uint8_t SendData0[SEND_DATA_NUM]; |
OGA | 0:4c7c138f3891 | 226 | |
OGA | 0:4c7c138f3891 | 227 | void dev_tx() |
OGA | 0:4c7c138f3891 | 228 | { |
OGA | 0:4c7c138f3891 | 229 | static uint8_t count = 0; |
OGA | 0:4c7c138f3891 | 230 | |
OGA | 0:4c7c138f3891 | 231 | SendData0[0] = KEYCODE; |
OGA | 0:4c7c138f3891 | 232 | |
OGA | 0:4c7c138f3891 | 233 | device.putc(SendData0[count]); |
OGA | 0:4c7c138f3891 | 234 | |
OGA | 0:4c7c138f3891 | 235 | count++; |
OGA | 0:4c7c138f3891 | 236 | |
OGA | 0:4c7c138f3891 | 237 | if(count > SEND_DATA_NUM){ |
OGA | 0:4c7c138f3891 | 238 | count = 0; |
OGA | 0:4c7c138f3891 | 239 | } |
OGA | 0:4c7c138f3891 | 240 | |
OGA | 0:4c7c138f3891 | 241 | led[4] = 1; |
OGA | 0:4c7c138f3891 | 242 | } |
OGA | 0:4c7c138f3891 | 243 | |
OGA | 0:4c7c138f3891 | 244 | |
OGA | 0:4c7c138f3891 | 245 | |
OGA | 0:4c7c138f3891 | 246 | |
OGA | 0:4c7c138f3891 | 247 | |
OGA | 0:4c7c138f3891 | 248 | int main() { |
OGA | 0:4c7c138f3891 | 249 | //init |
OGA | 0:4c7c138f3891 | 250 | |
OGA | 0:4c7c138f3891 | 251 | /*device.baud(9600); |
OGA | 0:4c7c138f3891 | 252 | device.printf("START"); |
OGA | 0:4c7c138f3891 | 253 | device.attach(&dev_tx,Serial::TxIrq);*/ |
OGA | 0:4c7c138f3891 | 254 | |
OGA | 0:4c7c138f3891 | 255 | //rivisedate(); |
OGA | 0:4c7c138f3891 | 256 | wait(3); |
OGA | 0:4c7c138f3891 | 257 | |
OGA | 0:4c7c138f3891 | 258 | interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない? |
OGA | 0:4c7c138f3891 | 259 | //sw1.mode(PullUp); |
OGA | 0:4c7c138f3891 | 260 | //init end |
OGA | 0:4c7c138f3891 | 261 | |
OGA | 0:4c7c138f3891 | 262 | while(1) { |
OGA | 0:4c7c138f3891 | 263 | SendData0[1] = jumI; |
OGA | 0:4c7c138f3891 | 264 | |
OGA | 0:4c7c138f3891 | 265 | pc.printf("R:%d G:%d B:%d %t moving_ave():%d %t robotFront():%d %t jumpInstruction():%d\n", redp[2], greenp[2], bluep[2], cAve, roboF, jumI); |
OGA | 0:4c7c138f3891 | 266 | |
OGA | 0:4c7c138f3891 | 267 | //jumpAction(jumI); |
OGA | 0:4c7c138f3891 | 268 | } |
OGA | 0:4c7c138f3891 | 269 | } |