jump!
Dependencies: ColorSensor1 HMC6352 Servo TextLCD mbed
main.cpp@0:feefc7aaa114, 2013-09-23 (annotated)
- Committer:
- OGA
- Date:
- Mon Sep 23 06:35:28 2013 +0000
- Revision:
- 0:feefc7aaa114
var.4.3
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OGA | 0:feefc7aaa114 | 1 | #include "mbed.h" |
OGA | 0:feefc7aaa114 | 2 | #include "ColorSensor.h" |
OGA | 0:feefc7aaa114 | 3 | #include "TextLCD.h" |
OGA | 0:feefc7aaa114 | 4 | #include "Servo.h" |
OGA | 0:feefc7aaa114 | 5 | #include "HMC6352.h" |
OGA | 0:feefc7aaa114 | 6 | |
OGA | 0:feefc7aaa114 | 7 | #include "main.h" |
OGA | 0:feefc7aaa114 | 8 | |
OGA | 0:feefc7aaa114 | 9 | |
OGA | 0:feefc7aaa114 | 10 | void tic_sensor() |
OGA | 0:feefc7aaa114 | 11 | { |
OGA | 0:feefc7aaa114 | 12 | Ultrasonic(); |
OGA | 0:feefc7aaa114 | 13 | |
OGA | 0:feefc7aaa114 | 14 | colorUpdate(); |
OGA | 0:feefc7aaa114 | 15 | |
OGA | 0:feefc7aaa114 | 16 | /*lcd.cls(); |
OGA | 0:feefc7aaa114 | 17 | lcd.locate(0,0); |
OGA | 0:feefc7aaa114 | 18 | lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); |
OGA | 0:feefc7aaa114 | 19 | */ |
OGA | 0:feefc7aaa114 | 20 | } |
OGA | 0:feefc7aaa114 | 21 | |
OGA | 0:feefc7aaa114 | 22 | ////////////////////////////////////////移動関数////////////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 23 | ///////////////////////////////////////////////////////////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 24 | void move(int vl,int vs){ |
OGA | 0:feefc7aaa114 | 25 | double fut_R,fut_L,true_vs; |
OGA | 0:feefc7aaa114 | 26 | |
OGA | 0:feefc7aaa114 | 27 | true_vs = abs(vs)/SPIN; |
OGA | 0:feefc7aaa114 | 28 | |
OGA | 0:feefc7aaa114 | 29 | if(true_vs > 40){ |
OGA | 0:feefc7aaa114 | 30 | vl = 0; |
OGA | 0:feefc7aaa114 | 31 | vs = 100*(vs/abs(vs)); |
OGA | 0:feefc7aaa114 | 32 | } |
OGA | 0:feefc7aaa114 | 33 | |
OGA | 0:feefc7aaa114 | 34 | fut_R = Convert_dekaruto((vl + vs)); |
OGA | 0:feefc7aaa114 | 35 | fut_L = Convert_dekaruto((vl - vs)/**1.4*/); |
OGA | 0:feefc7aaa114 | 36 | |
OGA | 0:feefc7aaa114 | 37 | fut_R = Convert_dekaruto(-50); |
OGA | 0:feefc7aaa114 | 38 | fut_L = Convert_dekaruto(-50); |
OGA | 0:feefc7aaa114 | 39 | |
OGA | 0:feefc7aaa114 | 40 | servoR = fut_R; |
OGA | 0:feefc7aaa114 | 41 | servoL = fut_L; |
OGA | 0:feefc7aaa114 | 42 | |
OGA | 0:feefc7aaa114 | 43 | //printf("R:%lf L:%lf\n",fut_R,fut_L); |
OGA | 0:feefc7aaa114 | 44 | //printf("R:%d L:%d\n",(vl + vs),-(vl - vs)); |
OGA | 0:feefc7aaa114 | 45 | } |
OGA | 0:feefc7aaa114 | 46 | |
OGA | 0:feefc7aaa114 | 47 | /*void PidUpdate() |
OGA | 0:feefc7aaa114 | 48 | { |
OGA | 0:feefc7aaa114 | 49 | inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0); |
OGA | 0:feefc7aaa114 | 50 | //printf("%lf\n",inputPID); |
OGA | 0:feefc7aaa114 | 51 | }*/ |
OGA | 0:feefc7aaa114 | 52 | |
OGA | 0:feefc7aaa114 | 53 | double vsOut(){ |
OGA | 0:feefc7aaa114 | 54 | double vs; |
OGA | 0:feefc7aaa114 | 55 | vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1; |
OGA | 0:feefc7aaa114 | 56 | vs = vs * 8; |
OGA | 0:feefc7aaa114 | 57 | |
OGA | 0:feefc7aaa114 | 58 | if(vs/abs(vs) < 0){ |
OGA | 0:feefc7aaa114 | 59 | //vs *= 1.3; |
OGA | 0:feefc7aaa114 | 60 | } |
OGA | 0:feefc7aaa114 | 61 | |
OGA | 0:feefc7aaa114 | 62 | if(abs(vs) > 90)vs = 90*(vs/abs(vs)); |
OGA | 0:feefc7aaa114 | 63 | if(abs(vs) < 25) vs = 25*(vs/abs(vs)); |
OGA | 0:feefc7aaa114 | 64 | |
OGA | 0:feefc7aaa114 | 65 | return vs; |
OGA | 0:feefc7aaa114 | 66 | } |
OGA | 0:feefc7aaa114 | 67 | |
OGA | 0:feefc7aaa114 | 68 | ////////////////////////////////////////超音波センサの//////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 69 | ////////////////////////////////////////スイッチ的な関数////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 70 | int ping_button(int ping,int button){ |
OGA | 0:feefc7aaa114 | 71 | static int continue_flag = 0; |
OGA | 0:feefc7aaa114 | 72 | static int change_flag = 0; |
OGA | 0:feefc7aaa114 | 73 | |
OGA | 0:feefc7aaa114 | 74 | if(continue_flag == 0){ |
OGA | 0:feefc7aaa114 | 75 | if(ping <= PINR_THR){ |
OGA | 0:feefc7aaa114 | 76 | ping_t.start(); |
OGA | 0:feefc7aaa114 | 77 | continue_flag = 1; |
OGA | 0:feefc7aaa114 | 78 | } |
OGA | 0:feefc7aaa114 | 79 | } |
OGA | 0:feefc7aaa114 | 80 | |
OGA | 0:feefc7aaa114 | 81 | if(continue_flag == 1){ |
OGA | 0:feefc7aaa114 | 82 | //agatterutoki |
OGA | 0:feefc7aaa114 | 83 | if(ping <= PINR_THR){ |
OGA | 0:feefc7aaa114 | 84 | if(change_flag == 0){ |
OGA | 0:feefc7aaa114 | 85 | if(ping_t.read_ms() >= 300){ |
OGA | 0:feefc7aaa114 | 86 | button = !button; |
OGA | 0:feefc7aaa114 | 87 | change_flag = 1; |
OGA | 0:feefc7aaa114 | 88 | } |
OGA | 0:feefc7aaa114 | 89 | } |
OGA | 0:feefc7aaa114 | 90 | } |
OGA | 0:feefc7aaa114 | 91 | //tatisagari |
OGA | 0:feefc7aaa114 | 92 | if(ping >= (PINR_THR+200)){ |
OGA | 0:feefc7aaa114 | 93 | ping_t.stop(); |
OGA | 0:feefc7aaa114 | 94 | ping_t.reset(); |
OGA | 0:feefc7aaa114 | 95 | continue_flag = 0; |
OGA | 0:feefc7aaa114 | 96 | change_flag = 0; |
OGA | 0:feefc7aaa114 | 97 | } |
OGA | 0:feefc7aaa114 | 98 | } |
OGA | 0:feefc7aaa114 | 99 | return button; |
OGA | 0:feefc7aaa114 | 100 | } |
OGA | 0:feefc7aaa114 | 101 | |
OGA | 0:feefc7aaa114 | 102 | ////////////////////////////////////////カラーセンサの//////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 103 | ////////////////////////////////////////補正プログラム//////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 104 | void rivisedate() |
OGA | 0:feefc7aaa114 | 105 | { |
OGA | 0:feefc7aaa114 | 106 | unsigned long red = 0,green = 0,blue =0; |
OGA | 0:feefc7aaa114 | 107 | static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; |
OGA | 0:feefc7aaa114 | 108 | |
OGA | 0:feefc7aaa114 | 109 | //最初の20回だけ平均を取る |
OGA | 0:feefc7aaa114 | 110 | for (int i=0;i<=20;i++){ |
OGA | 0:feefc7aaa114 | 111 | color0.getRGB(R[0],G[0],B[0]); |
OGA | 0:feefc7aaa114 | 112 | red += R[0] ; |
OGA | 0:feefc7aaa114 | 113 | green += G[0] ; |
OGA | 0:feefc7aaa114 | 114 | blue += B[0] ; |
OGA | 0:feefc7aaa114 | 115 | //pc.printf(" %d %d\n",ptm(sum),sum); |
OGA | 0:feefc7aaa114 | 116 | } |
OGA | 0:feefc7aaa114 | 117 | |
OGA | 0:feefc7aaa114 | 118 | rir = (double)green/ red ; |
OGA | 0:feefc7aaa114 | 119 | rib = (double)green/ blue ; |
OGA | 0:feefc7aaa114 | 120 | } |
OGA | 0:feefc7aaa114 | 121 | |
OGA | 0:feefc7aaa114 | 122 | void colorUpdate() |
OGA | 0:feefc7aaa114 | 123 | { |
OGA | 0:feefc7aaa114 | 124 | double colorSum[COLOR_NUM]; |
OGA | 0:feefc7aaa114 | 125 | unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; |
OGA | 0:feefc7aaa114 | 126 | |
OGA | 0:feefc7aaa114 | 127 | color0.getRGB(R[0],G[0],B[0]); |
OGA | 0:feefc7aaa114 | 128 | color1.getRGB(R[1],G[1],B[1]); |
OGA | 0:feefc7aaa114 | 129 | color2.getRGB(R[2],G[2],B[2]); |
OGA | 0:feefc7aaa114 | 130 | /*color3.getRGB(R[3],G[3],B[3]); |
OGA | 0:feefc7aaa114 | 131 | color4.getRGB(R[4],G[4],B[4]); |
OGA | 0:feefc7aaa114 | 132 | color5.getRGB(R[5],G[5],B[5]);*/ |
OGA | 0:feefc7aaa114 | 133 | |
OGA | 0:feefc7aaa114 | 134 | for (int i=0; i<COLOR_NUM; i++){ |
OGA | 0:feefc7aaa114 | 135 | colorSum[i] = R[i]*rir + G[i] + B[i]*rib ; |
OGA | 0:feefc7aaa114 | 136 | redp[i] = R[i]* rir * 100 / colorSum[i]; |
OGA | 0:feefc7aaa114 | 137 | greenp[i] = G[i] * 100 / colorSum[i]; |
OGA | 0:feefc7aaa114 | 138 | bluep[i] = B[i]* rib * 100 / colorSum[i]; |
OGA | 0:feefc7aaa114 | 139 | } |
OGA | 0:feefc7aaa114 | 140 | } |
OGA | 0:feefc7aaa114 | 141 | |
OGA | 0:feefc7aaa114 | 142 | ////////////////////////////////////////ジャンププログラム//////////////////////////////////// |
OGA | 0:feefc7aaa114 | 143 | /////////////////////////////////////////////////////////////////////////////////////////// |
OGA | 0:feefc7aaa114 | 144 | uint8_t jumpcondition() |
OGA | 0:feefc7aaa114 | 145 | { |
OGA | 0:feefc7aaa114 | 146 | uint8_t threshold = 0, t[3] = {0}; |
OGA | 0:feefc7aaa114 | 147 | |
OGA | 0:feefc7aaa114 | 148 | //青から赤に0.5秒以内に反応したらジャンプ |
OGA | 0:feefc7aaa114 | 149 | for(int i=0; i<COLOR_NUM; i++){ |
OGA | 0:feefc7aaa114 | 150 | if(bluep[i] >= B_THR){ |
OGA | 0:feefc7aaa114 | 151 | color_t[i].reset(); |
OGA | 0:feefc7aaa114 | 152 | color_t[i].start(); |
OGA | 0:feefc7aaa114 | 153 | t[i] = 0; |
OGA | 0:feefc7aaa114 | 154 | }else if(redp[i] >= R_THR){ |
OGA | 0:feefc7aaa114 | 155 | t[i] = color_t[i].read_ms(); |
OGA | 0:feefc7aaa114 | 156 | }else{ |
OGA | 0:feefc7aaa114 | 157 | t[i] = 0; |
OGA | 0:feefc7aaa114 | 158 | } |
OGA | 0:feefc7aaa114 | 159 | |
OGA | 0:feefc7aaa114 | 160 | if((t[i] <= 500) && (t[i] != 0)){ |
OGA | 0:feefc7aaa114 | 161 | threshold++; |
OGA | 0:feefc7aaa114 | 162 | } |
OGA | 0:feefc7aaa114 | 163 | } |
OGA | 0:feefc7aaa114 | 164 | |
OGA | 0:feefc7aaa114 | 165 | return threshold; |
OGA | 0:feefc7aaa114 | 166 | } |
OGA | 0:feefc7aaa114 | 167 | |
OGA | 0:feefc7aaa114 | 168 | void jumping(uint8_t threshold) |
OGA | 0:feefc7aaa114 | 169 | { |
OGA | 0:feefc7aaa114 | 170 | //超音波でジャンプのタイミング合わせる |
OGA | 0:feefc7aaa114 | 171 | if(threshold >= 1){ |
OGA | 0:feefc7aaa114 | 172 | jump_t.reset(); |
OGA | 0:feefc7aaa114 | 173 | jump_t.start(); |
OGA | 0:feefc7aaa114 | 174 | while(ultrasonicVal[0] < 1700){ |
OGA | 0:feefc7aaa114 | 175 | led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0; |
OGA | 0:feefc7aaa114 | 176 | air[0] = 1; air[1] = 0; |
OGA | 0:feefc7aaa114 | 177 | |
OGA | 0:feefc7aaa114 | 178 | if(jump_t.read_ms() > 1000)break; |
OGA | 0:feefc7aaa114 | 179 | } |
OGA | 0:feefc7aaa114 | 180 | led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1; |
OGA | 0:feefc7aaa114 | 181 | air[0] = 0; air[1] = 1; |
OGA | 0:feefc7aaa114 | 182 | wait(0.5); |
OGA | 0:feefc7aaa114 | 183 | }else{ |
OGA | 0:feefc7aaa114 | 184 | led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; |
OGA | 0:feefc7aaa114 | 185 | } |
OGA | 0:feefc7aaa114 | 186 | } |
OGA | 0:feefc7aaa114 | 187 | |
OGA | 0:feefc7aaa114 | 188 | |
OGA | 0:feefc7aaa114 | 189 | |
OGA | 0:feefc7aaa114 | 190 | |
OGA | 0:feefc7aaa114 | 191 | |
OGA | 0:feefc7aaa114 | 192 | |
OGA | 0:feefc7aaa114 | 193 | |
OGA | 0:feefc7aaa114 | 194 | |
OGA | 0:feefc7aaa114 | 195 | |
OGA | 0:feefc7aaa114 | 196 | |
OGA | 0:feefc7aaa114 | 197 | |
OGA | 0:feefc7aaa114 | 198 | int main() |
OGA | 0:feefc7aaa114 | 199 | { |
OGA | 0:feefc7aaa114 | 200 | rivisedate(); |
OGA | 0:feefc7aaa114 | 201 | |
OGA | 0:feefc7aaa114 | 202 | timer2.start(); |
OGA | 0:feefc7aaa114 | 203 | ping_t.start(); |
OGA | 0:feefc7aaa114 | 204 | |
OGA | 0:feefc7aaa114 | 205 | |
OGA | 0:feefc7aaa114 | 206 | //unsigned R, G, B; |
OGA | 0:feefc7aaa114 | 207 | int vl; |
OGA | 0:feefc7aaa114 | 208 | double vs; |
OGA | 0:feefc7aaa114 | 209 | uint8_t button, state=0; |
OGA | 0:feefc7aaa114 | 210 | |
OGA | 0:feefc7aaa114 | 211 | //pc.baud(115200); |
OGA | 0:feefc7aaa114 | 212 | air[0] = 0; air[1] = 1; |
OGA | 0:feefc7aaa114 | 213 | |
OGA | 0:feefc7aaa114 | 214 | |
OGA | 0:feefc7aaa114 | 215 | //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
OGA | 0:feefc7aaa114 | 216 | //pidUpdata.attach(&PidUpdate, PID_CYCLE); |
OGA | 0:feefc7aaa114 | 217 | interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない |
OGA | 0:feefc7aaa114 | 218 | |
OGA | 0:feefc7aaa114 | 219 | while(1) |
OGA | 0:feefc7aaa114 | 220 | { |
OGA | 0:feefc7aaa114 | 221 | |
OGA | 0:feefc7aaa114 | 222 | |
OGA | 0:feefc7aaa114 | 223 | |
OGA | 0:feefc7aaa114 | 224 | pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); |
OGA | 0:feefc7aaa114 | 225 | /* |
OGA | 0:feefc7aaa114 | 226 | lcd.cls(); |
OGA | 0:feefc7aaa114 | 227 | lcd.locate(0,0); |
OGA | 0:feefc7aaa114 | 228 | lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]); |
OGA | 0:feefc7aaa114 | 229 | lcd.locate(0,1); |
OGA | 0:feefc7aaa114 | 230 | lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]); |
OGA | 0:feefc7aaa114 | 231 | */ |
OGA | 0:feefc7aaa114 | 232 | //pc.printf("%d\n", ultrasonicVal[0]); |
OGA | 0:feefc7aaa114 | 233 | |
OGA | 0:feefc7aaa114 | 234 | |
OGA | 0:feefc7aaa114 | 235 | jumping(jumpcondition()); |
OGA | 0:feefc7aaa114 | 236 | |
OGA | 0:feefc7aaa114 | 237 | |
OGA | 0:feefc7aaa114 | 238 | |
OGA | 0:feefc7aaa114 | 239 | button = ping_button(ultrasonicVal[1],button); |
OGA | 0:feefc7aaa114 | 240 | |
OGA | 0:feefc7aaa114 | 241 | if(button){ |
OGA | 0:feefc7aaa114 | 242 | state = GO; |
OGA | 0:feefc7aaa114 | 243 | }else{ |
OGA | 0:feefc7aaa114 | 244 | state = STOP; |
OGA | 0:feefc7aaa114 | 245 | } |
OGA | 0:feefc7aaa114 | 246 | |
OGA | 0:feefc7aaa114 | 247 | |
OGA | 0:feefc7aaa114 | 248 | |
OGA | 0:feefc7aaa114 | 249 | |
OGA | 0:feefc7aaa114 | 250 | |
OGA | 0:feefc7aaa114 | 251 | if(state == GO){ |
OGA | 0:feefc7aaa114 | 252 | vl = -90;//led[0] = 1; led[1] = 1; |
OGA | 0:feefc7aaa114 | 253 | }else if(state == STOP){ |
OGA | 0:feefc7aaa114 | 254 | vl = 0;//led[0] = 0; led[1] = 0; |
OGA | 0:feefc7aaa114 | 255 | } |
OGA | 0:feefc7aaa114 | 256 | |
OGA | 0:feefc7aaa114 | 257 | vl = vl * STRAIGHT ; |
OGA | 0:feefc7aaa114 | 258 | vs = vsOut() * SPIN ; |
OGA | 0:feefc7aaa114 | 259 | |
OGA | 0:feefc7aaa114 | 260 | vl *= 0.5; |
OGA | 0:feefc7aaa114 | 261 | vs *= 0.3; |
OGA | 0:feefc7aaa114 | 262 | |
OGA | 0:feefc7aaa114 | 263 | move(vl,(int)vs); |
OGA | 0:feefc7aaa114 | 264 | } |
OGA | 0:feefc7aaa114 | 265 | } |