Example #1
0
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;
}