void Game_Class::switch_on_led( int led ) { printf("switch_on_led( %d )\n", led) ; reset_leds() ; switch(led) { case 1 : digitalWrite(led_A, HIGH); break ; case 2 : digitalWrite(led_B, HIGH); break ; case 3 : digitalWrite(led_C, HIGH); break ; case 4 : digitalWrite(led_D, HIGH); break ; default : break; } delay(500) ; reset_leds() ; }
void blink(int led) { // turn off all leds reset_leds(); // blink the resquested led for (int i=0;i<3;i++){ digitalWrite(led, HIGH); // turn on led delay(500); digitalWrite(led, LOW); // turn off led delay(500); } }
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; }