Arduino Mega 2560 Serial Input Cycle Problem

Projects Programming
21

Sorry that I tried to simplify the code for discussion. The following code is successfully run Mega with 'C' command every 1 second but failed for 100 ms.

#include <Wire.h>
#include <NewPing.h>
#define SONAR_NUM 3                      // Number of Sonar
#define MAX_DISTANCE 400                  // Maximum distance (in cm) to ping
NewPing sonar[SONAR_NUM] = {              // Ultrasonic Sensor Object array
  NewPing(13, 12, MAX_DISTANCE),
  NewPing(11, 10, MAX_DISTANCE),
  NewPing(9, 8, MAX_DISTANCE)
};

uint8_t i;
char inbyte;           // command byte received from Rpi (Serial 1)
String sid;           // String of Ultrasonic sensor id
String dist;          // String of distance value
String outpackage;    // String sent to rpi via Serial 1
String irsensor;      // String of ir sensor id
String irstate;       // ir sensor state
// unsigned int iteration;
unsigned int duration;
unsigned int distance;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial1.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
  inbyte = ' ';

  if (Serial.available() > 0){
    inbyte = Serial.read(); 
  }
  if (inbyte == 'C'){
    for (i = 0; i < SONAR_NUM; i++){
//      delay(10);
      sid = String(i + 1);
      duration = sonar[i].ping();      
      distance = (duration/2)*0.0343;
      distance = sonar[i].ping_cm();
      dist = String(distance);
      outpackage = "u"+sid+" "+dist+"\n";
      Serial.print(outpackage);
      Serial1.print(outpackage);
    }    
    Serial.print("E\n");
  } else if (inbyte == ' '){
    // do nothing 
  } else {
    Serial.write(inbyte);
//    Serial1.write(inbyte);
  }
}