Ryo Ogata / Mbed 2 deprecated jumpROBO5_0

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?

UserRevisionLine numberNew 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 }