ooooooooga
Dependencies: ColorSensor1 HMC6352 PID QEI Servo TextLCD mbed
main.cpp@0:e6bb0bcba274, 2013-10-01 (annotated)
- Committer:
- OGA
- Date:
- Tue Oct 01 07:44:16 2013 +0000
- Revision:
- 0:e6bb0bcba274
oooooga
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OGA | 0:e6bb0bcba274 | 1 | #include "mbed.h" |
OGA | 0:e6bb0bcba274 | 2 | #include "PID.h" |
OGA | 0:e6bb0bcba274 | 3 | #include "QEI.h" |
OGA | 0:e6bb0bcba274 | 4 | #include "Servo.h" |
OGA | 0:e6bb0bcba274 | 5 | #include "HMC6352.h" |
OGA | 0:e6bb0bcba274 | 6 | #include "ColorSensor.h" |
OGA | 0:e6bb0bcba274 | 7 | #include "TextLCD.h" |
OGA | 0:e6bb0bcba274 | 8 | |
OGA | 0:e6bb0bcba274 | 9 | #include "main.h" |
OGA | 0:e6bb0bcba274 | 10 | |
OGA | 0:e6bb0bcba274 | 11 | |
OGA | 0:e6bb0bcba274 | 12 | |
OGA | 0:e6bb0bcba274 | 13 | |
OGA | 0:e6bb0bcba274 | 14 | |
OGA | 0:e6bb0bcba274 | 15 | void tic_sensor() |
OGA | 0:e6bb0bcba274 | 16 | { |
OGA | 0:e6bb0bcba274 | 17 | Ultrasonic(); |
OGA | 0:e6bb0bcba274 | 18 | |
OGA | 0:e6bb0bcba274 | 19 | colorUpdate(); |
OGA | 0:e6bb0bcba274 | 20 | |
OGA | 0:e6bb0bcba274 | 21 | /*lcd.cls(); |
OGA | 0:e6bb0bcba274 | 22 | lcd.locate(0,0); |
OGA | 0:e6bb0bcba274 | 23 | lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); |
OGA | 0:e6bb0bcba274 | 24 | */ |
OGA | 0:e6bb0bcba274 | 25 | } |
OGA | 0:e6bb0bcba274 | 26 | |
OGA | 0:e6bb0bcba274 | 27 | |
OGA | 0:e6bb0bcba274 | 28 | |
OGA | 0:e6bb0bcba274 | 29 | |
OGA | 0:e6bb0bcba274 | 30 | |
OGA | 0:e6bb0bcba274 | 31 | ////////////////////////////////////////移動関数////////////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 32 | ///////////////////////////////////////////////////////////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 33 | void PidUpdate(){ |
OGA | 0:e6bb0bcba274 | 34 | static int state = WAIT,past_state = WAIT,next_state = WAIT; |
OGA | 0:e6bb0bcba274 | 35 | |
OGA | 0:e6bb0bcba274 | 36 | if(!mode_comp){ |
OGA | 0:e6bb0bcba274 | 37 | if(vl && !vs){ |
OGA | 0:e6bb0bcba274 | 38 | state = STRAIGHT; |
OGA | 0:e6bb0bcba274 | 39 | } |
OGA | 0:e6bb0bcba274 | 40 | if(vs){ |
OGA | 0:e6bb0bcba274 | 41 | state = TURN; |
OGA | 0:e6bb0bcba274 | 42 | } |
OGA | 0:e6bb0bcba274 | 43 | } |
OGA | 0:e6bb0bcba274 | 44 | if((state != past_state)){ |
OGA | 0:e6bb0bcba274 | 45 | mode_comp = 1; |
OGA | 0:e6bb0bcba274 | 46 | |
OGA | 0:e6bb0bcba274 | 47 | if(process == 0){ |
OGA | 0:e6bb0bcba274 | 48 | if(abs(enkoda1) > 180 && abs(enkoda1) < 240){ |
OGA | 0:e6bb0bcba274 | 49 | process++; |
OGA | 0:e6bb0bcba274 | 50 | } |
OGA | 0:e6bb0bcba274 | 51 | } |
OGA | 0:e6bb0bcba274 | 52 | if(process == 1){ |
OGA | 0:e6bb0bcba274 | 53 | if(state == STRAIGHT){ |
OGA | 0:e6bb0bcba274 | 54 | vl = 10; |
OGA | 0:e6bb0bcba274 | 55 | vs = 0; |
OGA | 0:e6bb0bcba274 | 56 | } |
OGA | 0:e6bb0bcba274 | 57 | if(state == TURN){ |
OGA | 0:e6bb0bcba274 | 58 | vl = 0; |
OGA | 0:e6bb0bcba274 | 59 | vs = 10; |
OGA | 0:e6bb0bcba274 | 60 | } |
OGA | 0:e6bb0bcba274 | 61 | if(abs(vc) < 60){ |
OGA | 0:e6bb0bcba274 | 62 | vc_count++; |
OGA | 0:e6bb0bcba274 | 63 | } |
OGA | 0:e6bb0bcba274 | 64 | if(vc_count > 2){ |
OGA | 0:e6bb0bcba274 | 65 | vc_count = 0; |
OGA | 0:e6bb0bcba274 | 66 | mode_comp = 0; |
OGA | 0:e6bb0bcba274 | 67 | process = 0; |
OGA | 0:e6bb0bcba274 | 68 | } |
OGA | 0:e6bb0bcba274 | 69 | } |
OGA | 0:e6bb0bcba274 | 70 | } |
OGA | 0:e6bb0bcba274 | 71 | |
OGA | 0:e6bb0bcba274 | 72 | enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0); |
OGA | 0:e6bb0bcba274 | 73 | enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0); |
OGA | 0:e6bb0bcba274 | 74 | |
OGA | 0:e6bb0bcba274 | 75 | if((!vs)&&(mode_comp == 0)){ |
OGA | 0:e6bb0bcba274 | 76 | pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses()); |
OGA | 0:e6bb0bcba274 | 77 | }else if((vs)&&(mode_comp == 0)){ |
OGA | 0:e6bb0bcba274 | 78 | pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses()); |
OGA | 0:e6bb0bcba274 | 79 | } |
OGA | 0:e6bb0bcba274 | 80 | |
OGA | 0:e6bb0bcba274 | 81 | if((!vs)&&(mode_comp)){ |
OGA | 0:e6bb0bcba274 | 82 | pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses()); |
OGA | 0:e6bb0bcba274 | 83 | }else if((vs)&&(mode_comp)){ |
OGA | 0:e6bb0bcba274 | 84 | pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses()); |
OGA | 0:e6bb0bcba274 | 85 | } |
OGA | 0:e6bb0bcba274 | 86 | |
OGA | 0:e6bb0bcba274 | 87 | vc = (int)pid.compute(); |
OGA | 0:e6bb0bcba274 | 88 | |
OGA | 0:e6bb0bcba274 | 89 | printf("vc:%lf\n",vc); |
OGA | 0:e6bb0bcba274 | 90 | |
OGA | 0:e6bb0bcba274 | 91 | double fut_R = 0.5,fut_L = 0.5; |
OGA | 0:e6bb0bcba274 | 92 | |
OGA | 0:e6bb0bcba274 | 93 | if(abs(vc) > 250){ |
OGA | 0:e6bb0bcba274 | 94 | vc = 0; |
OGA | 0:e6bb0bcba274 | 95 | } |
OGA | 0:e6bb0bcba274 | 96 | |
OGA | 0:e6bb0bcba274 | 97 | int vlR = 0; |
OGA | 0:e6bb0bcba274 | 98 | int vlL = 0; |
OGA | 0:e6bb0bcba274 | 99 | int vcR = vc; |
OGA | 0:e6bb0bcba274 | 100 | int vcL = vc; |
OGA | 0:e6bb0bcba274 | 101 | |
OGA | 0:e6bb0bcba274 | 102 | vlR = -vs; |
OGA | 0:e6bb0bcba274 | 103 | vlL = vs; |
OGA | 0:e6bb0bcba274 | 104 | |
OGA | 0:e6bb0bcba274 | 105 | vl *= 0.5; |
OGA | 0:e6bb0bcba274 | 106 | vc *= 0.5; |
OGA | 0:e6bb0bcba274 | 107 | |
OGA | 0:e6bb0bcba274 | 108 | vlR *= 0.4; |
OGA | 0:e6bb0bcba274 | 109 | vlL *= 0.4; |
OGA | 0:e6bb0bcba274 | 110 | |
OGA | 0:e6bb0bcba274 | 111 | vcR *= 0.6; |
OGA | 0:e6bb0bcba274 | 112 | vcL *= 0.6; |
OGA | 0:e6bb0bcba274 | 113 | |
OGA | 0:e6bb0bcba274 | 114 | |
OGA | 0:e6bb0bcba274 | 115 | if(!vs){ |
OGA | 0:e6bb0bcba274 | 116 | if(vl > 0){ |
OGA | 0:e6bb0bcba274 | 117 | fut_R = Convert_dekaruto((int)(vl - vc)); |
OGA | 0:e6bb0bcba274 | 118 | fut_L = Convert_dekaruto((int)(vl * 0.95 + vc)); |
OGA | 0:e6bb0bcba274 | 119 | } |
OGA | 0:e6bb0bcba274 | 120 | if(vl < 0){ |
OGA | 0:e6bb0bcba274 | 121 | vc *= -1; |
OGA | 0:e6bb0bcba274 | 122 | fut_R = Convert_dekaruto((int)(vl * 0.95 + vc)); |
OGA | 0:e6bb0bcba274 | 123 | fut_L = Convert_dekaruto((int)(vl - vc)); |
OGA | 0:e6bb0bcba274 | 124 | } |
OGA | 0:e6bb0bcba274 | 125 | }else{ |
OGA | 0:e6bb0bcba274 | 126 | if(vlR < 0){ |
OGA | 0:e6bb0bcba274 | 127 | vcR *= -1; |
OGA | 0:e6bb0bcba274 | 128 | } |
OGA | 0:e6bb0bcba274 | 129 | |
OGA | 0:e6bb0bcba274 | 130 | if(vlL < 0){ |
OGA | 0:e6bb0bcba274 | 131 | vcL *= -1; |
OGA | 0:e6bb0bcba274 | 132 | } |
OGA | 0:e6bb0bcba274 | 133 | if(vs > 0){ |
OGA | 0:e6bb0bcba274 | 134 | fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR)); |
OGA | 0:e6bb0bcba274 | 135 | fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL)); |
OGA | 0:e6bb0bcba274 | 136 | } |
OGA | 0:e6bb0bcba274 | 137 | |
OGA | 0:e6bb0bcba274 | 138 | if(vs < 0){ |
OGA | 0:e6bb0bcba274 | 139 | fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR)); |
OGA | 0:e6bb0bcba274 | 140 | fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL)); |
OGA | 0:e6bb0bcba274 | 141 | } |
OGA | 0:e6bb0bcba274 | 142 | } |
OGA | 0:e6bb0bcba274 | 143 | |
OGA | 0:e6bb0bcba274 | 144 | servoR = fut_R; |
OGA | 0:e6bb0bcba274 | 145 | servoL = fut_L; |
OGA | 0:e6bb0bcba274 | 146 | |
OGA | 0:e6bb0bcba274 | 147 | if(!mode_comp){ |
OGA | 0:e6bb0bcba274 | 148 | past_state = state; |
OGA | 0:e6bb0bcba274 | 149 | } |
OGA | 0:e6bb0bcba274 | 150 | } |
OGA | 0:e6bb0bcba274 | 151 | |
OGA | 0:e6bb0bcba274 | 152 | void CompassUpdate(){ |
OGA | 0:e6bb0bcba274 | 153 | inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0); |
OGA | 0:e6bb0bcba274 | 154 | } |
OGA | 0:e6bb0bcba274 | 155 | |
OGA | 0:e6bb0bcba274 | 156 | double vssOut(){ |
OGA | 0:e6bb0bcba274 | 157 | double vss; |
OGA | 0:e6bb0bcba274 | 158 | vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0; |
OGA | 0:e6bb0bcba274 | 159 | |
OGA | 0:e6bb0bcba274 | 160 | if(abs(vss) < 10){ |
OGA | 0:e6bb0bcba274 | 161 | vss = 0; |
OGA | 0:e6bb0bcba274 | 162 | } |
OGA | 0:e6bb0bcba274 | 163 | |
OGA | 0:e6bb0bcba274 | 164 | vss *= 3.0; |
OGA | 0:e6bb0bcba274 | 165 | |
OGA | 0:e6bb0bcba274 | 166 | |
OGA | 0:e6bb0bcba274 | 167 | if((vss)&&(abs(vss) < 500)){ |
OGA | 0:e6bb0bcba274 | 168 | vss += (vss/abs(vss)) * 500; |
OGA | 0:e6bb0bcba274 | 169 | } |
OGA | 0:e6bb0bcba274 | 170 | |
OGA | 0:e6bb0bcba274 | 171 | if(abs(vss) > 1000){ |
OGA | 0:e6bb0bcba274 | 172 | vss = 1000 * (vss/abs(vss)); |
OGA | 0:e6bb0bcba274 | 173 | } |
OGA | 0:e6bb0bcba274 | 174 | |
OGA | 0:e6bb0bcba274 | 175 | return vss; |
OGA | 0:e6bb0bcba274 | 176 | } |
OGA | 0:e6bb0bcba274 | 177 | |
OGA | 0:e6bb0bcba274 | 178 | void move(int vll,int vss){ |
OGA | 0:e6bb0bcba274 | 179 | if(!swR){ |
OGA | 0:e6bb0bcba274 | 180 | wheel1.reset(); |
OGA | 0:e6bb0bcba274 | 181 | } |
OGA | 0:e6bb0bcba274 | 182 | |
OGA | 0:e6bb0bcba274 | 183 | if(!swL){ |
OGA | 0:e6bb0bcba274 | 184 | wheel2.reset(); |
OGA | 0:e6bb0bcba274 | 185 | } |
OGA | 0:e6bb0bcba274 | 186 | |
OGA | 0:e6bb0bcba274 | 187 | vl = vll; |
OGA | 0:e6bb0bcba274 | 188 | vs = vss; |
OGA | 0:e6bb0bcba274 | 189 | } |
OGA | 0:e6bb0bcba274 | 190 | |
OGA | 0:e6bb0bcba274 | 191 | |
OGA | 0:e6bb0bcba274 | 192 | ////////////////////////////////////////超音波センサの//////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 193 | ////////////////////////////////////////スイッチ的な関数////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 194 | int ping_button(int ping,int button){ |
OGA | 0:e6bb0bcba274 | 195 | static int continue_flag = 0; |
OGA | 0:e6bb0bcba274 | 196 | static int change_flag = 0; |
OGA | 0:e6bb0bcba274 | 197 | |
OGA | 0:e6bb0bcba274 | 198 | if(continue_flag == 0){ |
OGA | 0:e6bb0bcba274 | 199 | if(ping <= PINR_THR){ |
OGA | 0:e6bb0bcba274 | 200 | ping_t.start(); |
OGA | 0:e6bb0bcba274 | 201 | continue_flag = 1; |
OGA | 0:e6bb0bcba274 | 202 | } |
OGA | 0:e6bb0bcba274 | 203 | } |
OGA | 0:e6bb0bcba274 | 204 | |
OGA | 0:e6bb0bcba274 | 205 | if(continue_flag == 1){ |
OGA | 0:e6bb0bcba274 | 206 | //agatterutoki |
OGA | 0:e6bb0bcba274 | 207 | if(ping <= PINR_THR){ |
OGA | 0:e6bb0bcba274 | 208 | if(change_flag == 0){ |
OGA | 0:e6bb0bcba274 | 209 | if(ping_t.read_ms() >= 150){ |
OGA | 0:e6bb0bcba274 | 210 | button = !button; |
OGA | 0:e6bb0bcba274 | 211 | change_flag = 1; |
OGA | 0:e6bb0bcba274 | 212 | } |
OGA | 0:e6bb0bcba274 | 213 | } |
OGA | 0:e6bb0bcba274 | 214 | } |
OGA | 0:e6bb0bcba274 | 215 | //tatisagari |
OGA | 0:e6bb0bcba274 | 216 | if(ping >= (PINR_THR+200)){ |
OGA | 0:e6bb0bcba274 | 217 | ping_t.stop(); |
OGA | 0:e6bb0bcba274 | 218 | ping_t.reset(); |
OGA | 0:e6bb0bcba274 | 219 | continue_flag = 0; |
OGA | 0:e6bb0bcba274 | 220 | change_flag = 0; |
OGA | 0:e6bb0bcba274 | 221 | } |
OGA | 0:e6bb0bcba274 | 222 | } |
OGA | 0:e6bb0bcba274 | 223 | return button; |
OGA | 0:e6bb0bcba274 | 224 | } |
OGA | 0:e6bb0bcba274 | 225 | |
OGA | 0:e6bb0bcba274 | 226 | |
OGA | 0:e6bb0bcba274 | 227 | ////////////////////////////////////////カラーセンサの//////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 228 | ////////////////////////////////////////補正プログラム//////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 229 | void rivisedate() |
OGA | 0:e6bb0bcba274 | 230 | { |
OGA | 0:e6bb0bcba274 | 231 | unsigned long red = 0,green = 0,blue =0; |
OGA | 0:e6bb0bcba274 | 232 | static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; |
OGA | 0:e6bb0bcba274 | 233 | |
OGA | 0:e6bb0bcba274 | 234 | //最初の20回だけ平均を取る |
OGA | 0:e6bb0bcba274 | 235 | for (int i=0;i<=20;i++){ |
OGA | 0:e6bb0bcba274 | 236 | color0.getRGB(R[0],G[0],B[0]); |
OGA | 0:e6bb0bcba274 | 237 | red += R[0] ; |
OGA | 0:e6bb0bcba274 | 238 | green += G[0] ; |
OGA | 0:e6bb0bcba274 | 239 | blue += B[0] ; |
OGA | 0:e6bb0bcba274 | 240 | //pc.printf(" %d %d\n",ptm(sum),sum); |
OGA | 0:e6bb0bcba274 | 241 | } |
OGA | 0:e6bb0bcba274 | 242 | |
OGA | 0:e6bb0bcba274 | 243 | rir = (double)green/ red ; |
OGA | 0:e6bb0bcba274 | 244 | rib = (double)green/ blue ; |
OGA | 0:e6bb0bcba274 | 245 | } |
OGA | 0:e6bb0bcba274 | 246 | |
OGA | 0:e6bb0bcba274 | 247 | void colorUpdate() |
OGA | 0:e6bb0bcba274 | 248 | { |
OGA | 0:e6bb0bcba274 | 249 | double colorSum[COLOR_NUM]; |
OGA | 0:e6bb0bcba274 | 250 | unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; |
OGA | 0:e6bb0bcba274 | 251 | |
OGA | 0:e6bb0bcba274 | 252 | color0.getRGB(R[0],G[0],B[0]); |
OGA | 0:e6bb0bcba274 | 253 | color1.getRGB(R[1],G[1],B[1]); |
OGA | 0:e6bb0bcba274 | 254 | color2.getRGB(R[2],G[2],B[2]); |
OGA | 0:e6bb0bcba274 | 255 | /*color3.getRGB(R[3],G[3],B[3]); |
OGA | 0:e6bb0bcba274 | 256 | color4.getRGB(R[4],G[4],B[4]); |
OGA | 0:e6bb0bcba274 | 257 | color5.getRGB(R[5],G[5],B[5]);*/ |
OGA | 0:e6bb0bcba274 | 258 | |
OGA | 0:e6bb0bcba274 | 259 | for (int i=0; i<COLOR_NUM; i++){ |
OGA | 0:e6bb0bcba274 | 260 | colorSum[i] = R[i]*rir + G[i] + B[i]*rib ; |
OGA | 0:e6bb0bcba274 | 261 | redp[i] = R[i]* rir * 100 / colorSum[i]; |
OGA | 0:e6bb0bcba274 | 262 | greenp[i] = G[i] * 100 / colorSum[i]; |
OGA | 0:e6bb0bcba274 | 263 | bluep[i] = B[i]* rib * 100 / colorSum[i]; |
OGA | 0:e6bb0bcba274 | 264 | } |
OGA | 0:e6bb0bcba274 | 265 | } |
OGA | 0:e6bb0bcba274 | 266 | |
OGA | 0:e6bb0bcba274 | 267 | |
OGA | 0:e6bb0bcba274 | 268 | ////////////////////////////////////////ジャンププログラム//////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 269 | /////////////////////////////////////////////////////////////////////////////////////////// |
OGA | 0:e6bb0bcba274 | 270 | uint8_t jumpcondition() |
OGA | 0:e6bb0bcba274 | 271 | { |
OGA | 0:e6bb0bcba274 | 272 | uint8_t threshold = 0, t[3] = {0}; |
OGA | 0:e6bb0bcba274 | 273 | |
OGA | 0:e6bb0bcba274 | 274 | //青から赤に0.5秒以内に反応したらジャンプ |
OGA | 0:e6bb0bcba274 | 275 | for(int i=0; i<COLOR_NUM; i++){ |
OGA | 0:e6bb0bcba274 | 276 | if(bluep[i] >= B_THR){ |
OGA | 0:e6bb0bcba274 | 277 | color_t[i].reset(); |
OGA | 0:e6bb0bcba274 | 278 | color_t[i].start(); |
OGA | 0:e6bb0bcba274 | 279 | t[i] = 0; |
OGA | 0:e6bb0bcba274 | 280 | }else if(redp[i] >= R_THR){ |
OGA | 0:e6bb0bcba274 | 281 | t[i] = color_t[i].read_ms(); |
OGA | 0:e6bb0bcba274 | 282 | }else{ |
OGA | 0:e6bb0bcba274 | 283 | t[i] = 0; |
OGA | 0:e6bb0bcba274 | 284 | } |
OGA | 0:e6bb0bcba274 | 285 | |
OGA | 0:e6bb0bcba274 | 286 | if((t[i] <= 500) && (t[i] != 0)){ |
OGA | 0:e6bb0bcba274 | 287 | threshold++; |
OGA | 0:e6bb0bcba274 | 288 | } |
OGA | 0:e6bb0bcba274 | 289 | } |
OGA | 0:e6bb0bcba274 | 290 | |
OGA | 0:e6bb0bcba274 | 291 | return threshold; |
OGA | 0:e6bb0bcba274 | 292 | } |
OGA | 0:e6bb0bcba274 | 293 | |
OGA | 0:e6bb0bcba274 | 294 | void jumping(uint8_t threshold) |
OGA | 0:e6bb0bcba274 | 295 | { |
OGA | 0:e6bb0bcba274 | 296 | //超音波でジャンプのタイミング合わせる |
OGA | 0:e6bb0bcba274 | 297 | if(threshold >= 1){ |
OGA | 0:e6bb0bcba274 | 298 | jump_t.reset(); |
OGA | 0:e6bb0bcba274 | 299 | jump_t.start(); |
OGA | 0:e6bb0bcba274 | 300 | while(ultrasonicVal[0] < 1700){ |
OGA | 0:e6bb0bcba274 | 301 | led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0; |
OGA | 0:e6bb0bcba274 | 302 | air[0] = 1; air[1] = 0; |
OGA | 0:e6bb0bcba274 | 303 | |
OGA | 0:e6bb0bcba274 | 304 | if(jump_t.read_ms() > 1000)break; |
OGA | 0:e6bb0bcba274 | 305 | } |
OGA | 0:e6bb0bcba274 | 306 | led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1; |
OGA | 0:e6bb0bcba274 | 307 | air[0] = 0; air[1] = 1; |
OGA | 0:e6bb0bcba274 | 308 | wait(0.5); |
OGA | 0:e6bb0bcba274 | 309 | }else{ |
OGA | 0:e6bb0bcba274 | 310 | led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; |
OGA | 0:e6bb0bcba274 | 311 | } |
OGA | 0:e6bb0bcba274 | 312 | } |
OGA | 0:e6bb0bcba274 | 313 | |
OGA | 0:e6bb0bcba274 | 314 | |
OGA | 0:e6bb0bcba274 | 315 | uint8_t robostop() |
OGA | 0:e6bb0bcba274 | 316 | { |
OGA | 0:e6bb0bcba274 | 317 | if(bluep[1] >= B_THR){ |
OGA | 0:e6bb0bcba274 | 318 | return 1; |
OGA | 0:e6bb0bcba274 | 319 | }else{ |
OGA | 0:e6bb0bcba274 | 320 | return 0; |
OGA | 0:e6bb0bcba274 | 321 | } |
OGA | 0:e6bb0bcba274 | 322 | } |
OGA | 0:e6bb0bcba274 | 323 | |
OGA | 0:e6bb0bcba274 | 324 | |
OGA | 0:e6bb0bcba274 | 325 | |
OGA | 0:e6bb0bcba274 | 326 | |
OGA | 0:e6bb0bcba274 | 327 | |
OGA | 0:e6bb0bcba274 | 328 | |
OGA | 0:e6bb0bcba274 | 329 | |
OGA | 0:e6bb0bcba274 | 330 | |
OGA | 0:e6bb0bcba274 | 331 | |
OGA | 0:e6bb0bcba274 | 332 | |
OGA | 0:e6bb0bcba274 | 333 | |
OGA | 0:e6bb0bcba274 | 334 | int main() { |
OGA | 0:e6bb0bcba274 | 335 | rivisedate(); |
OGA | 0:e6bb0bcba274 | 336 | wait(3); |
OGA | 0:e6bb0bcba274 | 337 | |
OGA | 0:e6bb0bcba274 | 338 | timer2.start(); |
OGA | 0:e6bb0bcba274 | 339 | ping_t.start(); |
OGA | 0:e6bb0bcba274 | 340 | |
OGA | 0:e6bb0bcba274 | 341 | pid.setInputLimits(MINIMUM,MAXIMUM); |
OGA | 0:e6bb0bcba274 | 342 | pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); |
OGA | 0:e6bb0bcba274 | 343 | pid.setBias(PID_BIAS); |
OGA | 0:e6bb0bcba274 | 344 | pid.setMode(AUTO_MODE); |
OGA | 0:e6bb0bcba274 | 345 | pid.setSetPoint(REFERENCE); |
OGA | 0:e6bb0bcba274 | 346 | |
OGA | 0:e6bb0bcba274 | 347 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
OGA | 0:e6bb0bcba274 | 348 | compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE); |
OGA | 0:e6bb0bcba274 | 349 | |
OGA | 0:e6bb0bcba274 | 350 | swR.mode(PullUp); |
OGA | 0:e6bb0bcba274 | 351 | swL.mode(PullUp); |
OGA | 0:e6bb0bcba274 | 352 | |
OGA | 0:e6bb0bcba274 | 353 | air[0] = 0; air[1] = 1; |
OGA | 0:e6bb0bcba274 | 354 | |
OGA | 0:e6bb0bcba274 | 355 | int setR = 0,setL = 0; |
OGA | 0:e6bb0bcba274 | 356 | int vll = 0,vss = 0; |
OGA | 0:e6bb0bcba274 | 357 | |
OGA | 0:e6bb0bcba274 | 358 | servoR = 0.595; |
OGA | 0:e6bb0bcba274 | 359 | servoL = 0.59; |
OGA | 0:e6bb0bcba274 | 360 | |
OGA | 0:e6bb0bcba274 | 361 | while(1){ |
OGA | 0:e6bb0bcba274 | 362 | if(!swR){ |
OGA | 0:e6bb0bcba274 | 363 | setR = 1; |
OGA | 0:e6bb0bcba274 | 364 | } |
OGA | 0:e6bb0bcba274 | 365 | |
OGA | 0:e6bb0bcba274 | 366 | if(!swL){ |
OGA | 0:e6bb0bcba274 | 367 | setL = 1; |
OGA | 0:e6bb0bcba274 | 368 | } |
OGA | 0:e6bb0bcba274 | 369 | |
OGA | 0:e6bb0bcba274 | 370 | if(setR){ |
OGA | 0:e6bb0bcba274 | 371 | servoR = 0.5; |
OGA | 0:e6bb0bcba274 | 372 | } |
OGA | 0:e6bb0bcba274 | 373 | |
OGA | 0:e6bb0bcba274 | 374 | if(setL){ |
OGA | 0:e6bb0bcba274 | 375 | servoL = 0.5; |
OGA | 0:e6bb0bcba274 | 376 | } |
OGA | 0:e6bb0bcba274 | 377 | |
OGA | 0:e6bb0bcba274 | 378 | if(setR && setL){ |
OGA | 0:e6bb0bcba274 | 379 | break; |
OGA | 0:e6bb0bcba274 | 380 | } |
OGA | 0:e6bb0bcba274 | 381 | |
OGA | 0:e6bb0bcba274 | 382 | wait(0.1); |
OGA | 0:e6bb0bcba274 | 383 | } |
OGA | 0:e6bb0bcba274 | 384 | |
OGA | 0:e6bb0bcba274 | 385 | wheel1.reset(); |
OGA | 0:e6bb0bcba274 | 386 | wheel2.reset(); |
OGA | 0:e6bb0bcba274 | 387 | |
OGA | 0:e6bb0bcba274 | 388 | interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない |
OGA | 0:e6bb0bcba274 | 389 | pidUpdata.attach(&PidUpdate, PID_CYCLE); |
OGA | 0:e6bb0bcba274 | 390 | |
OGA | 0:e6bb0bcba274 | 391 | uint8_t button, state=0; |
OGA | 0:e6bb0bcba274 | 392 | |
OGA | 0:e6bb0bcba274 | 393 | while(1) { |
OGA | 0:e6bb0bcba274 | 394 | vll = 0; |
OGA | 0:e6bb0bcba274 | 395 | vss = 0; |
OGA | 0:e6bb0bcba274 | 396 | |
OGA | 0:e6bb0bcba274 | 397 | pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); |
OGA | 0:e6bb0bcba274 | 398 | jumping(jumpcondition()); |
OGA | 0:e6bb0bcba274 | 399 | |
OGA | 0:e6bb0bcba274 | 400 | |
OGA | 0:e6bb0bcba274 | 401 | |
OGA | 0:e6bb0bcba274 | 402 | button = ping_button(ultrasonicVal[1],button); |
OGA | 0:e6bb0bcba274 | 403 | |
OGA | 0:e6bb0bcba274 | 404 | if(button){ |
OGA | 0:e6bb0bcba274 | 405 | state = GO; |
OGA | 0:e6bb0bcba274 | 406 | }else{ |
OGA | 0:e6bb0bcba274 | 407 | state = STOP; |
OGA | 0:e6bb0bcba274 | 408 | } |
OGA | 0:e6bb0bcba274 | 409 | |
OGA | 0:e6bb0bcba274 | 410 | |
OGA | 0:e6bb0bcba274 | 411 | |
OGA | 0:e6bb0bcba274 | 412 | |
OGA | 0:e6bb0bcba274 | 413 | |
OGA | 0:e6bb0bcba274 | 414 | if(state == GO){ |
OGA | 0:e6bb0bcba274 | 415 | vll = 700;//led[0] = 1; led[1] = 1; |
OGA | 0:e6bb0bcba274 | 416 | }else if(state == STOP){ |
OGA | 0:e6bb0bcba274 | 417 | vll = -700;//led[0] = 0; led[1] = 0; |
OGA | 0:e6bb0bcba274 | 418 | } |
OGA | 0:e6bb0bcba274 | 419 | |
OGA | 0:e6bb0bcba274 | 420 | move(vll,vss); |
OGA | 0:e6bb0bcba274 | 421 | } |
OGA | 0:e6bb0bcba274 | 422 | } |