int main() { char s[30],s1[30]; int i,j,n,x,y; while (scanf("%d",&n)!=EOF) { for (i=0; i<n; i++) { a[i].num=1; a[i].block[1]=i; postion[i]=i; } while (scanf("%s",&s)) { if (strcmp(s,"quit")==0) break; scanf("%d%s%d",&x,&s1,&y); if ((postion[x]!=postion[y])&&(x!=y)) { if ((strcmp(s,"move")==0)&&(strcmp(s1,"onto")==0)) { turnback(x); turnback(y); move1(x,y); } if ((strcmp(s,"move")==0)&&(strcmp(s1,"over")==0)) { turnback(x); move1(x,y); } if ((strcmp(s,"pile")==0)&&(strcmp(s1,"onto")==0)) { turnback(y); move2(x,y); } if ((strcmp(s,"pile")==0)&&(strcmp(s1,"over")==0)) { move2(x,y); } } } for (i=0; i<n; i++) { printf("%d:",i); for (j=1; j<=a[i].num; j++) printf(" %d",a[i].block[j]); printf("\n"); } } return 0; }
/*__________________________________________________________________________________________*/ CPoint wzq::AiGo(bool color) { CPoint turnback(-1,-1); clear(false); //pc全部置为255 fillin(color,false,true); fillin(!color,false,false); wzq::aitype2 = getAItype(); unsigned char found = 255; for (int ix = 0; ix < 15; ix++) { for (int iy = 0; iy < 15; iy++) { for (int im = 0; im <= 3; im++) { if (pc[ix][iy][im] <= found) { found = pc[ix][iy][im]; turnback.x = ix,turnback.y = iy; } } } } if (found <= 23) return turnback; else return getmax(color); }
task autonomous() { motor393 motor_[10];//declaro los motores short fronts[10]={1, FRONTLEFT, FRONTRIGHT, FRONTOMNI, FRONTSUC, FRONTLIFTFRONT, FRONTLIFTBACK, FRONTSHOTRIGHT, FRONTSHOTLEFT, 1}; short ports[10]={0,motorLeft,motorRight,motorOmni,motorSuc,motorLiftFront,motorLiftBack,motorShotRight,motorShotLeft,0}; initialize(&motor_[0], &fronts[0], &ports[0]); startSpeed(&motor_[0]); up(&motor_[motorShotRight], &motor_[motorShotLeft]); front(motor_); delay(8000); stopRobot(motor_); motorFront(&motor_[motorSuc]); delay(2000); stopLift(&motor_[motorShotRight], &motor_[motorShotLeft]); back(motor_); delay(500); turnback(motor_); delay(2600); while(true){ stopLift(&motor_[motorShotRight], &motor_[motorShotLeft]); front(motor_); } }
int robot_command (uint16_t *cmd, uint16_t *resp, uint8_t *resplen) { unsigned long start = 0; uint8_t infolen = 0; int checkdir; int motor_state_save; int error = -1; int ret = SUCCESS; lcd.clear(); // clear LCD reset_leds(); // turn off all leds digitalWrite(Led_Blue, HIGH); // turn on led blue switch (cmd[0]) { case CMD_STOP: Serial.println("CMD_STOP"); lcd.print("STOP"); stop(); motor_state = STATE_STOP; *resplen = 0; break; case CMD_START: if (cmd[1] == 0) { Serial.println("CMD_START"); lcd.print("START"); start_forward(); } else { Serial.print("CMD_START_TEST motor: "); Serial.println((int)cmd[1]); lcd.print("START motor: "); lcd.print((int)cmd[1]); start_forward_test(cmd[1]); } motor_state = STATE_GO; *resplen = 0; break; case CMD_CHECK_AROUND: Serial.println("CMD_CHECK_AROUND"); lcd.print("CHECK AROUND"); checkdir = check_around(); lcd.setCursor(0,1); if (checkdir == DIRECTION_LEFT) lcd.print("LEFT"); else if (checkdir == DIRECTION_RIGHT) lcd.print("RIGHT"); else if (checkdir == OBSTACLE_LEFT) lcd.print("OBSTACLE LEFT"); else if (checkdir == OBSTACLE_RIGHT) lcd.print("OBSTACLE RIGHT"); else if (checkdir == OBSTACLE) lcd.print("OBSTACLE"); else lcd.print("?"); resp[0] = checkdir; *resplen = 0+1; break; case CMD_MOVE_TILT_PAN: Serial.print("CMD_MOVE_TILT_PAN: "); Serial.print((int)cmd[1]);Serial.print((int)cmd[2]);Serial.print((int)cmd[3]);Serial.println((int)cmd[4]); if (cmd[2] == 0) HPos = (int)cmd[1] + 90; else HPos = 90 - (int)cmd[1]; if (cmd[4] == 0) VPos = (int)cmd[3] + 90; else VPos = 90 - (int)cmd[3]; Serial.print("CMD_MOVE_TILT_PAN, HPos VPos: "); Serial.print(HPos);Serial.println(VPos); lcd.print("MOVE TILT&PAN"); lcd.setCursor(0,1); lcd.print("X: "); lcd.print(HPos); lcd.print(" Y: "); lcd.print(VPos); TiltPan_move(HPos, VPos); *resplen = 0; break; case CMD_TURN: if (cmd[1] == 180) { Serial.print("CMD_TURN_BACK"); lcd.print("TURN BACK "); ret = turnback (10); // 10s max if (ret != SUCCESS){ Serial.print("turnback error"); Serial.println(ret); lcd.setCursor(0,1); lcd.print("turnback error: "); lcd.print(ret); error = 1; } } else if (motor_state == STATE_GO) { Serial.print("CMD_TURN, alpha: "); Serial.print((cmd[2] != 1) ? ('+'):('-')); Serial.println((int)cmd[1]); lcd.print("TURN"); lcd.print((cmd[2] != 1) ? ('+'):('-')); lcd.print((int)cmd[1]);lcd.print((char)223); //degree ret = turn ((double)((cmd[2] != 1) ? (cmd[1]):(-cmd[1])), 5); // 5s max if (ret != SUCCESS){ Serial.print("turn error"); Serial.println(ret); lcd.setCursor(0,1); lcd.print(" turn error: "); lcd.print(ret); error = 1; } } *resplen = 0; break; case CMD_INFOS: Serial.println("CMD_INFOS"); ret = infos (resp, &infolen); if (resp[MOTOR_STATE] == STATE_GO) { lcd.print("RUNING"); } else { lcd.print("STOPPED"); } lcd.setCursor(0,1); lcd.print((int)resp[TEMPERATURE]); lcd.print((byte)lcd_celcius);lcd.write(lcd_pipe); lcd.print((int)resp[DISTANCE]); lcd.print("cm");lcd.write(lcd_pipe); lcd.print((int)resp[DIRECTION]); lcd.print((char)223); //degree *resplen = infolen; break; case CMD_PICTURE: Serial.print("CMD_PICTURE, no_picture: "); no_picture++; Serial.println(no_picture); lcd.print("PICTURE "); motor_state_save = motor_state; if (motor_state == STATE_GO) { Serial.println("Stop"); stop(); motor_state = STATE_STOP; } ret = JPEGCamera.makePicture (no_picture); if (ret == SUCCESS) { lcd.setCursor(0,1); lcd.print("picture: "); lcd.print(no_picture); } else { Serial.print("makePicture error: "); Serial.println(ret); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); error = 1; } if (motor_state_save == STATE_GO) { Serial.println("Start"); start_forward(); motor_state = STATE_GO; } // byte 0: picture number resp[0] = no_picture; *resplen = 0+1; break; case CMD_ALERT: Serial.println("CMD_ALERT"); lcd.print("Alert"); blink(Led_Blue); buzz(5); // If motor_state == STATE_GO => Stop if (motor_state == STATE_GO) { Serial.println("Stop"); stop(); motor_state = STATE_STOP; } // Make 3 pictures left, front and right if ((HPos != 90) || (VPos !=90)) { HPos = 90; VPos = 90; TiltPan_move(HPos, VPos); } Serial.print("makePicture, no_picture: "); no_picture++; Serial.println(no_picture); lcd.print("PICTURE "); ret = JPEGCamera.makePicture (no_picture); if (ret == SUCCESS) { lcd.setCursor(0,1); lcd.print("picture: "); lcd.print(no_picture); } else { Serial.print("makePicture error: "); Serial.println(ret); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); error = 1; } if (ret == SUCCESS) { HPos = 0; VPos = 90; TiltPan_move(HPos, VPos); Serial.print("makePicture, no_picture: "); no_picture++; Serial.println(no_picture); lcd.print("PICTURE "); ret = JPEGCamera.makePicture (no_picture); lcd.setCursor(0,1); lcd.print("picture: "); lcd.print(no_picture); } else { Serial.print("makePicture error: "); Serial.println(ret); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); error = 1; } if (ret == SUCCESS) { HPos = 180; VPos = 90; TiltPan_move(HPos, VPos); Serial.print("makePicture, no_picture: "); no_picture++; Serial.println(no_picture); lcd.print("PICTURE "); ret = JPEGCamera.makePicture (no_picture); lcd.setCursor(0,1); lcd.print("picture: "); lcd.print(no_picture); } else { Serial.print("makePicture error: "); Serial.println(ret); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); error = 1; } // byte 0: last picture number resp[0] = no_picture; *resplen = 0+1; HPos = 90; VPos = 90; TiltPan_move(HPos, VPos); break; case CMD_CHECK: Serial.println("CMD_CHECK"); lcd.print("Check"); alert_status = check(); if (alert_status != SUCCESS) { Serial.print("Alert detected: ");Serial.println(alert_status); lcd.setCursor(0,1); lcd.print("Alert: "); lcd.print(alert_status); } else { Serial.print("No alert detected: "); lcd.setCursor(0,1); lcd.print(" No Alert"); } // byte 0: alert resp[0] = alert_status; *resplen = 0+1; break; case CMD_GO: Serial.print("CMD_GO, nb seconds: "); Serial.print((int)cmd[1]); lcd.print("GO ");lcd.print((int)cmd[1]);lcd.print("secs"); if (motor_state != STATE_GO) { Serial.println("start_forward"); start_forward(); motor_state = STATE_GO; } error = -1; GOtimeout = (unsigned long)cmd[1]; start = millis(); while((millis() - start < GOtimeout*1000) && (error == -1)) { ret = go(GOtimeout); if ((ret != SUCCESS) && (ret != OBSTACLE) && (ret != OBSTACLE_LEFT) && (ret != OBSTACLE_RIGHT)) { stop(); motor_state = STATE_STOP; error = 1; Serial.print("CMD_GO error: "); Serial.println(ret); Serial.println("Stop"); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); } else if ((ret == OBSTACLE) || (ret == OBSTACLE_LEFT) || (ret == OBSTACLE_RIGHT)) { stop(); motor_state = STATE_STOP; buzz(3); blink(Led_Red); Serial.print("CMD_GO Obstacle: ");Serial.println(ret); Serial.println("Stop"); lcd.setCursor(0,1); if (ret == OBSTACLE_LEFT) lcd.print("OBSTACLE LEFT"); else if (ret == OBSTACLE_RIGHT) lcd.print("OBSTACLE RIGHT"); else if (ret == OBSTACLE) lcd.print("OBSTACLE"); else ret = SUCCESS; checkdir = check_around(); Serial.print("check_around, direction: "); Serial.println(checkdir); lcd.clear(); lcd.print("check around"); lcd.setCursor(0,1); if (checkdir == DIRECTION_LEFT) lcd.print("LEFT"); else if (checkdir == DIRECTION_RIGHT) lcd.print("RIGHT"); else if (checkdir == OBSTACLE_LEFT) lcd.print("OBSTACLE LEFT"); else if (checkdir == OBSTACLE_RIGHT) lcd.print("OBSTACLE RIGHT"); else if (checkdir == OBSTACLE) lcd.print("OBSTACLE"); else lcd.print("?");; if (checkdir == DIRECTION_LEFT) { start_forward(); motor_state = STATE_GO; ret = turn (-45, 5); // turn -45 degrees during 5s max if (ret != SUCCESS) { stop(); motor_state = STATE_STOP; error = 1; Serial.print("turn error: "); Serial.println(ret); Serial.println("Stop"); lcd.clear(); lcd.print("turn left"); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); } else { lcd.clear(); lcd.print("turn left OK"); } } else if (checkdir == DIRECTION_RIGHT) { start_forward(); motor_state = STATE_GO; ret = turn (+45, 5); // turn +45 degrees during 5s max if (ret != SUCCESS) { stop(); motor_state = STATE_STOP; error = 1; Serial.print("turn error: "); Serial.println(ret); Serial.println("Stop"); lcd.clear(); lcd.print("turn right"); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); } else { lcd.clear(); lcd.print("turn right OK"); } } else { buzz(3); blink(Led_Red); motor_state = STATE_GO; ret = turnback (10); // turn back during 10s max if (ret != SUCCESS) { stop(); motor_state = STATE_STOP; error = 1; Serial.print("turnback error"); Serial.println(ret); Serial.println("Stop"); lcd.clear(); lcd.print("turnback"); lcd.setCursor(0,1); lcd.print("error: "); lcd.print(ret); } else { lcd.clear(); lcd.print("turnback OK"); } } } else { Serial.println("GO OK"); lcd.clear(); lcd.print("GO OK"); error = 0; } } // end while ret = infos (resp, &infolen); if (error == 0) { if (resp[MOTOR_STATE] == STATE_GO) { lcd.print("RUNNING"); } else { lcd.print("STOPPED"); } lcd.setCursor(0,1); lcd.print((int)resp[TEMPERATURE]); lcd.print((byte)lcd_celcius);lcd.write(lcd_pipe); lcd.print((int)resp[DISTANCE]); lcd.print("cm");lcd.write(lcd_pipe); lcd.print((int)resp[DIRECTION]); lcd.print((char)223); //degree } *resplen = infolen; break; case CMD_PI: Serial.print("CMD_PI activated, type: "); Serial.print((int)cmd[1]); Serial.print(" - freq: ");Serial.println((int)cmd[2]); lcd.print("PI activated ");lcd.print((int)cmd[1]); PI_activated = (int)cmd[1]; if (PI_activated <> PI_NO_COMM) PI_freqInfos = (int)cmd[2]* 1000; Serial.print("PI_activated: "); Serial.println(PI_activated); Serial.print("PI_freqInfos: ");Serial.println(PI_freqInfos); *resplen = 0; break; default: Serial.println("invalid command"); lcd.print("invalid command"); *resplen = 0; break; } //end switch if (error == 1) { blink(Led_Red); blink(Led_Red); buzz(7); *resplen = 0; } reset_leds(); // turn off all leds return ret; }
/*_____________________________________________________________________________________*/ CPoint wzq::getmax(bool color) { CPoint turnback(0,0); setall(); begin: bool x = 0; int fx = 0, fy = 1; for (int ix = 0; ix < 15; ix++) { for (int iy = 0; iy <15; iy++) { if (wzq::nicest[ix][iy] == 0) { if (get_victory2(ix,iy,fx,fy,false) == true) fx = ix,fy = iy,x = 1; } } } //找最大 if (x == 0) { for (int ix = 0; ix < 15; ix++) { for (int iy =0; iy < 15; iy++) { if (wzq::nicest[ix][iy] != 1/*不输*/) { if (get_victory2(ix,iy,turnback.x,turnback.y,false) == true) turnback.x = ix,turnback.y = iy,x = 1; } } } if (x==0) { for (int ix = 0; ix <= 14; ix++) { for (int iy = 0; iy <= 14; iy++) { if (get_victory2(ix,iy,turnback.x,turnback.y,false) == true) turnback.x=ix,turnback.y=iy,x=1; } } } if (x==0) turnback.x =7,turnback.y =7; return turnback; }//找完 for (ix = 0; ix < 15; ix++) { for (int iy = 0; iy < 15; iy++) { wzq::trygo[ix][iy] = wzq::map[ix][iy]; } } //填 wzq::checkgo(fx,fy, color,1,fx,fy); //预测 if (nicest[fx][fy] == 2)//win { turnback.x = fx,turnback.y = fy; return turnback; } else { goto begin;//重来一次 } //判断 }